Merge remote-tracking branch cros/main into factory-brya-14909.124.B-main

Generated by: util/update_release_branch.py --baseboard brya --relevant_paths_file baseboard/brya/relevant-paths.txt factory-brya-14909.124.B-main Relevant changes: git log --oneline 19d4d68ffa..aa40b859b3 -- baseboard/brya board/agah board/anahera board/banshee board/brya board/crota board/felwinter board/gimble board/kano board/mithrax board/osiris board/primus board/redrix board/taeko board/taniks board/vell board/volmar driver/bc12/pi3usb9201_public.* driver/charger/bq25710.* driver/ppc/nx20p348x.* driver/ppc/syv682x_public.* driver/retimer/bb_retimer_public.* driver/tcpm/nct38xx.* driver/tcpm/ps8xxx_public.* driver/tcpm/tcpci.* include/power/alderlake* include/intel_x86.h power/alderlake* power/intel_x86.c util/getversion.sh e6da633c38 driver: Sort header files 234a87ae2d tcpci: Add FRS enable to driver structure a56be59ccd tcpm_header: add test for tcpm_dump_registers 57b3256963 Rename CONFIG_CHARGER_INPUT_CURRENT to _CHARGER_DEFAULT_CURRENT_LIMIT e420c8ff9a marasov: Modify TypeC and TypeA configuration. 43b53e0045 Add default implementation of board_set_charge_limit b75dc90677 Add CONFIG_CHARGER_MIN_INPUT_CURRENT_LIMIT f1b563c350 baseboard: Sort header files 7d01b1e58d driver/retimer/ps8818.h: Add I2C ADDR FLAGS 0x30, 0x58, 0x70 ec31407993 Add CONFIG_CHARGER_INPUT_CURRENT_DERATE_PCT 8f89f69a5b crota: disable lid angle sensor for clamshell BRANCH=None BUG=b:260630630 b:163093572 b:259002141 b:255184961 b:259354679 BUG=b:247100970 b:254328661 TEST=`emerge-brya chromeos-ec` Force-Relevant-Builds: all Change-Id: I0ecfa0e6af68631283c7a9e8f1afb9d827176c62 Signed-off-by: YH Lin <yueherngl@google.com>
author: YH Lin <yueherngl@google.com> 2022-11-30 21:45:30 +0000
committer: YH Lin <yueherngl@google.com> 2022-11-30 21:45:30 +0000
commit: 184d13e77614be3be5374d3fef9d1edf66ec8687 (patch)
tree: d0c89ae8814c2ba35c238c6c0644ec6b1602c63a /common
parent: 19d4d68ffa8b6910d716ab5e1953c41b58614a57 (diff)
parent: aa40b859b3a73e5a205bc561c1a29eff38485461 (diff)
download: chrome-ec-184d13e77614be3be5374d3fef9d1edf66ec8687.tar.gz
111 files changed, 1278 insertions, 725 deletions
diff --git a/common/accel_cal.c b/common/accel_cal.c
index 0fe9fe7656..878f3b06fa 100644
--- a/common/accel_cal.c
+++ b/common/accel_cal.c
@@ -3,9 +3,9 @@
  * found in the LICENSE file.
  */
 
+#include "accel_cal.h"
 #include "common.h"
 #include "console.h"
-#include "accel_cal.h"
 
 #define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ##args)
 
diff --git a/common/acpi.c b/common/acpi.c
index c6fcfce417..5dd71d244e 100644
--- a/common/acpi.c
+++ b/common/acpi.c
@@ -5,6 +5,7 @@
 
 #include "acpi.h"
 #include "battery.h"
+#include "body_detection.h"
 #include "common.h"
 #include "console.h"
 #include "dptf.h"
@@ -17,8 +18,8 @@
 #include "lpc.h"
 #include "printf.h"
 #include "pwm.h"
-#include "timer.h"
 #include "tablet_mode.h"
+#include "timer.h"
 #include "usb_charge.h"
 #include "usb_common.h"
 #include "util.h"
@@ -235,6 +236,11 @@ int acpi_ap_to_ec(int is_cmd, uint8_t value, uint8_t *resultptr)
 				   EC_ACPI_MEM_DDPN_MASK)
 				  << EC_ACPI_MEM_DDPN_SHIFT;
 #endif
+
+#ifdef CONFIG_BODY_DETECTION_NOTIFY_MODE_CHANGE
+			if (body_detect_get_state() == BODY_DETECTION_ON_BODY)
+				result |= BIT(EC_ACPI_MEM_STTB_SHIFT);
+#endif
 			break;
 
 		case EC_ACPI_MEM_DEVICE_FEATURES0:
diff --git a/common/base32.c b/common/base32.c
index fc3fe3c8ae..e5c30c4d6f 100644
--- a/common/base32.c
+++ b/common/base32.c
@@ -5,8 +5,8 @@
 
 /* Base-32 encoding/decoding */
 
-#include "common.h"
 #include "base32.h"
+#include "common.h"
 #include "util.h"
 
 static const unsigned char crc5_table1[] = { 0x00, 0x0E, 0x1C, 0x12, 0x11, 0x1F,
diff --git a/common/base_state.c b/common/base_state.c
index f90a5e7ce5..7bbd14eb00 100644
--- a/common/base_state.c
+++ b/common/base_state.c
@@ -5,8 +5,8 @@
 
 #include "base_state.h"
 #include "console.h"
-#include "host_command.h"
 #include "hooks.h"
+#include "host_command.h"
 
 #define CPRINTS(format, args...) cprints(CC_MOTION_LID, format, ##args)
 
diff --git a/common/battery_fuel_gauge.c b/common/battery_fuel_gauge.c
index 88f941f194..6a62c21479 100644
--- a/common/battery_fuel_gauge.c
+++ b/common/battery_fuel_gauge.c
@@ -7,6 +7,7 @@
 
 #include "battery_fuel_gauge.h"
 #include "battery_smart.h"
+#include "builtin/assert.h"
 #include "console.h"
 #include "hooks.h"
 #include "i2c.h"
@@ -22,7 +23,8 @@
  * a device name has been specified in the board_battery_info table,
  * then both the manufacturer and device name must match.
  */
-static bool authenticate_battery_type(int index, char *manuf_name)
+test_export_static bool authenticate_battery_type(int index,
+						  const char *manuf_name)
 {
 	char device_name[32];
 
@@ -84,6 +86,12 @@ void battery_set_fixed_battery_type(int type)
 }
 #endif /* CONFIG_BATTERY_TYPE_NO_AUTO_DETECT */
 
+/**
+ * Allows us to override the battery in order to select the battery which has
+ * the right configuration for the test.
+ */
+test_export_static int battery_fuel_gauge_type_override = -1;
+
 /* Get type of the battery connected on the board */
 static int get_battery_type(void)
 {
@@ -91,6 +99,10 @@ static int get_battery_type(void)
 	int i;
 	static enum battery_type battery_type = BATTERY_TYPE_COUNT;
 
+	if (IS_ENABLED(TEST_BUILD) && battery_fuel_gauge_type_override >= 0) {
+		return battery_fuel_gauge_type_override;
+	}
+
 	/*
 	 * If battery_type is not the default value, then can return here
 	 * as there is no need to query the fuel gauge.
@@ -237,17 +249,12 @@ enum ec_error_list battery_sleep_fuel_gauge(void)
 	return sb_write(sleep_command->reg_addr, sleep_command->reg_data);
 }
 
-static enum ec_error_list battery_get_fet_status_regval(int *regval)
+static enum ec_error_list battery_get_fet_status_regval(int type, int *regval)
 {
 	int rv;
 	uint8_t data[6];
-	int type = get_battery_type();
 
-	/* If battery type is not known, can't check CHG/DCHG FETs */
-	if (type == BATTERY_TYPE_COUNT) {
-		/* Still don't know, so return here */
-		return EC_ERROR_BUSY;
-	}
+	ASSERT(type < BATTERY_TYPE_COUNT);
 
 	/* Read the status of charge/discharge FETs */
 	if (board_battery_info[type].fuel_gauge.fet.mfgacc_support == 1) {
@@ -270,7 +277,7 @@ int battery_is_charge_fet_disabled(void)
 	int type = get_battery_type();
 
 	/* If battery type is not known, can't check CHG/DCHG FETs */
-	if (type == BATTERY_TYPE_COUNT) {
+	if (type >= BATTERY_TYPE_COUNT) {
 		/* Still don't know, so return here */
 		return -1;
 	}
@@ -281,7 +288,7 @@ int battery_is_charge_fet_disabled(void)
 	if (!board_battery_info[type].fuel_gauge.fet.cfet_mask)
 		return 0;
 
-	rv = battery_get_fet_status_regval(&reg);
+	rv = battery_get_fet_status_regval(type, &reg);
 	if (rv)
 		return -1;
 
@@ -307,12 +314,12 @@ enum battery_disconnect_state battery_get_disconnect_state(void)
 	int type = get_battery_type();
 
 	/* If battery type is not known, can't check CHG/DCHG FETs */
-	if (type == BATTERY_TYPE_COUNT) {
+	if (type >= BATTERY_TYPE_COUNT) {
 		/* Still don't know, so return here */
 		return BATTERY_DISCONNECT_ERROR;
 	}
 
-	if (battery_get_fet_status_regval(&reg))
+	if (battery_get_fet_status_regval(type, &reg))
 		return BATTERY_DISCONNECT_ERROR;
 
 	if ((reg & board_battery_info[type].fuel_gauge.fet.reg_mask) ==
diff --git a/common/battery_v2.c b/common/battery_v2.c
index 57ae07196b..50b5fb12f1 100644
--- a/common/battery_v2.c
+++ b/common/battery_v2.c
@@ -86,7 +86,7 @@ host_command_battery_get_static(struct host_cmd_handler_args *args)
 	const struct ec_params_battery_static_info *p = args->params;
 	const struct battery_static_info *bs;
 
-	if (p->index < 0 || p->index >= CONFIG_BATTERY_COUNT)
+	if (p->index >= CONFIG_BATTERY_COUNT)
 		return EC_RES_INVALID_PARAM;
 	bs = &battery_static[p->index];
 
@@ -144,7 +144,7 @@ host_command_battery_get_dynamic(struct host_cmd_handler_args *args)
 	const struct ec_params_battery_dynamic_info *p = args->params;
 	struct ec_response_battery_dynamic_info *r = args->response;
 
-	if (p->index < 0 || p->index >= CONFIG_BATTERY_COUNT)
+	if (p->index >= CONFIG_BATTERY_COUNT)
 		return EC_RES_INVALID_PARAM;
 
 	args->response_size = sizeof(*r);
diff --git a/common/bluetooth_le.c b/common/bluetooth_le.c
index 3108553a8b..d102740293 100644
--- a/common/bluetooth_le.c
+++ b/common/bluetooth_le.c
@@ -3,10 +3,10 @@
  * found in the LICENSE file.
  */
 
-#include "builtin/assert.h"
 #include "bluetooth_le.h"
-#include "util.h"
+#include "builtin/assert.h"
 #include "console.h"
+#include "util.h"
 
 #define CPRINTF(format, args...) cprintf(CC_BLUETOOTH_LE, format, ##args)
 
diff --git a/common/body_detection.c b/common/body_detection.c
index 848c4f08c1..e6b842ddf1 100644
--- a/common/body_detection.c
+++ b/common/body_detection.c
@@ -113,6 +113,11 @@ void body_detect_change_state(enum body_detect_states state, bool spoof)
 		/* reset time counting of stationary */
 		stationary_timeframe = 0;
 	}
+
+#ifdef CONFIG_BODY_DETECTION_NOTIFY_MODE_CHANGE
+	host_set_single_event(EC_HOST_EVENT_MODE_CHANGE);
+#endif
+
 	/* state changing log */
 	CPRINTS("body_detect changed state to: %s body",
 		motion_state ? "on" : "off");
diff --git a/common/btle_hci_controller.c b/common/btle_hci_controller.c
index a4dcadaccf..605b67945b 100644
--- a/common/btle_hci_controller.c
+++ b/common/btle_hci_controller.c
@@ -3,9 +3,9 @@
  * found in the LICENSE file.
  */
 
-#include "btle_hci_int.h"
-#include "btle_hci2.h"
 #include "bluetooth_le_ll.h"
+#include "btle_hci2.h"
+#include "btle_hci_int.h"
 #include "console.h"
 
 #ifdef CONFIG_BLUETOOTH_HCI_DEBUG
diff --git a/common/btle_ll.c b/common/btle_ll.c
index 71ca108565..eea9534fe3 100644
--- a/common/btle_ll.c
+++ b/common/btle_ll.c
@@ -3,15 +3,15 @@
  * found in the LICENSE file.
  */
 
-#include "bluetooth_le_ll.h"
 #include "bluetooth_le.h"
+#include "bluetooth_le_ll.h"
 #include "btle_hci_int.h"
-#include "util.h"
 #include "console.h"
 #include "radio.h"
 #include "radio_test.h"
 #include "task.h"
 #include "timer.h"
+#include "util.h"
 
 #ifdef CONFIG_BLUETOOTH_LL_DEBUG
 
diff --git a/common/button.c b/common/button.c
index aa7bcf6b4c..284d026285 100644
--- a/common/button.c
+++ b/common/button.c
@@ -12,8 +12,8 @@
 #include "compile_time_macros.h"
 #include "console.h"
 #include "gpio.h"
-#include "host_command.h"
 #include "hooks.h"
+#include "host_command.h"
 #include "keyboard_protocol.h"
 #include "led_common.h"
 #include "mkbp_input_devices.h"
diff --git a/common/charge_manager.c b/common/charge_manager.c
index 3e8b937e53..703b8933db 100644
--- a/common/charge_manager.c
+++ b/common/charge_manager.c
@@ -23,7 +23,7 @@
 #include "typec_control.h"
 #include "usb_common.h"
 #include "usb_pd.h"
-#include "usb_pd_dpm.h"
+#include "usb_pd_dpm_sm.h"
 #include "usb_pd_tcpm.h"
 #include "util.h"
 
@@ -835,8 +835,12 @@ static void charge_manager_refresh(void)
 		override_port = OVERRIDE_OFF;
 
 	if (new_supplier == CHARGE_SUPPLIER_NONE) {
+#ifdef CONFIG_CHARGER_DEFAULT_CURRENT_LIMIT
+		new_charge_current = CONFIG_CHARGER_DEFAULT_CURRENT_LIMIT;
+#else
 		new_charge_current = 0;
-		new_charge_current_uncapped = 0;
+#endif
+		new_charge_current_uncapped = new_charge_current;
 		new_charge_voltage = 0;
 	} else {
 		new_charge_current_uncapped =
@@ -1707,3 +1711,11 @@ board_fill_source_power_info(int port, struct ec_response_usb_pd_power_info *r)
 	r->meas.current_lim = 0;
 	r->max_power = 0;
 }
+
+__overridable void board_set_charge_limit(int port, int supplier, int charge_ma,
+					  int max_ma, int charge_mv)
+{
+#if defined(CONFIG_CHARGER) && defined(CONFIG_BATTERY)
+	charge_set_input_current_limit(charge_ma, charge_mv);
+#endif
+}
diff --git a/common/charge_state_v2.c b/common/charge_state_v2.c
index 728606ef8a..742c94bdb6 100644
--- a/common/charge_state_v2.c
+++ b/common/charge_state_v2.c
@@ -9,9 +9,9 @@
 #include "battery_smart.h"
 #include "builtin/assert.h"
 #include "charge_manager.h"
-#include "charger_profile_override.h"
 #include "charge_state.h"
 #include "charger.h"
+#include "charger_profile_override.h"
 #include "chipset.h"
 #include "common.h"
 #include "console.h"
@@ -1382,7 +1382,7 @@ struct charge_state_data *charge_get_status(void)
 }
 
 /* Determine if the battery is outside of allowable temperature range */
-static int battery_outside_charging_temperature(void)
+int battery_outside_charging_temperature(void)
 {
 	const struct battery_info *batt_info = battery_get_info();
 	int batt_temp_c = DECI_KELVIN_TO_CELSIUS(curr.batt.temperature);
@@ -1507,9 +1507,9 @@ static int get_desired_input_current(enum battery_present batt_present,
 		int ilim = charge_manager_get_charger_current();
 		return ilim == CHARGE_CURRENT_UNINITIALIZED ?
 			       CHARGE_CURRENT_UNINITIALIZED :
-			       MAX(CONFIG_CHARGER_INPUT_CURRENT, ilim);
+			       MAX(CONFIG_CHARGER_DEFAULT_CURRENT_LIMIT, ilim);
 #else
-		return CONFIG_CHARGER_INPUT_CURRENT;
+		return CONFIG_CHARGER_DEFAULT_CURRENT_LIMIT;
 #endif
 	} else {
 #ifdef CONFIG_USB_POWER_DELIVERY
@@ -2082,14 +2082,15 @@ int charge_want_shutdown(void)
 	       (curr.batt.state_of_charge < battery_level_shutdown);
 }
 
-int charge_prevent_power_on(int power_button_pressed)
+#ifdef CONFIG_CHARGER_MIN_BAT_PCT_FOR_POWER_ON
+test_export_static int charge_prevent_power_on_automatic_power_on = 1;
+#endif
+
+bool charge_prevent_power_on(bool power_button_pressed)
 {
 	int prevent_power_on = 0;
 	struct batt_params params;
 	struct batt_params *current_batt_params = &curr.batt;
-#ifdef CONFIG_CHARGER_MIN_BAT_PCT_FOR_POWER_ON
-	static int automatic_power_on = 1;
-#endif
 
 	/* If battery params seem uninitialized then retrieve them */
 	if (current_batt_params->is_present == BP_NOT_SURE) {
@@ -2104,7 +2105,7 @@ int charge_prevent_power_on(int power_button_pressed)
 	 * power-ups are user-requested and non-automatic.
 	 */
 	if (power_button_pressed)
-		automatic_power_on = 0;
+		charge_prevent_power_on_automatic_power_on = 0;
 	/*
 	 * Require a minimum battery level to power on and ensure that the
 	 * battery can provide power to the system.
@@ -2150,12 +2151,13 @@ int charge_prevent_power_on(int power_button_pressed)
 	 * except when auto-power-on at EC startup and the battery
 	 * is physically present.
 	 */
-	prevent_power_on &=
-		(system_is_locked() || (automatic_power_on
+	prevent_power_on &= (system_is_locked() ||
+			     (charge_prevent_power_on_automatic_power_on
 #ifdef CONFIG_BATTERY_HW_PRESENT_CUSTOM
-					&& battery_hw_present() == BP_YES
+
+			      && battery_hw_present() == BP_YES
 #endif
-					));
+			      ));
 #endif /* CONFIG_CHARGER_MIN_BAT_PCT_FOR_POWER_ON */
 
 #ifdef CONFIG_CHARGE_MANAGER
@@ -2189,7 +2191,7 @@ int charge_prevent_power_on(int power_button_pressed)
 		prevent_power_on = 1;
 #endif /* CONFIG_SYSTEM_UNLOCKED */
 
-	return prevent_power_on;
+	return prevent_power_on != 0;
 }
 
 static int battery_near_full(void)
@@ -2328,6 +2330,18 @@ int charge_set_input_current_limit(int ma, int mv)
 {
 	__maybe_unused int chgnum = 0;
 
+#ifdef CONFIG_CHARGER_INPUT_CURRENT_DERATE_PCT
+	if (CONFIG_CHARGER_INPUT_CURRENT_DERATE_PCT != 0) {
+		ma = (ma * (100 - CONFIG_CHARGER_INPUT_CURRENT_DERATE_PCT)) /
+		     100;
+	}
+#endif
+#ifdef CONFIG_CHARGER_MIN_INPUT_CURRENT_LIMIT
+	if (CONFIG_CHARGER_MIN_INPUT_CURRENT_LIMIT > 0) {
+		ma = MAX(ma, CONFIG_CHARGER_MIN_INPUT_CURRENT_LIMIT);
+	}
+#endif
+
 	if (IS_ENABLED(CONFIG_OCPC))
 		chgnum = charge_get_active_chg_chip();
 #ifdef CONFIG_EC_EC_COMM_BATTERY_CLIENT
diff --git a/common/chargen.c b/common/chargen.c
index 9b2c23ed79..626e13e71f 100644
--- a/common/chargen.c
+++ b/common/chargen.c
@@ -6,13 +6,13 @@
 #include "common.h"
 #include "config.h"
 #include "console.h"
+#include "hooks.h"
+#include "task.h"
 #include "timer.h"
 #include "uart.h"
 #include "usb_console.h"
 #include "util.h"
 #include "watchdog.h"
-#include "hooks.h"
-#include "task.h"
 
 #ifndef SECTION_IS_RO
 
diff --git a/common/charger.c b/common/charger.c
index db780d59cf..198dc0db8c 100644
--- a/common/charger.c
+++ b/common/charger.c
@@ -6,15 +6,16 @@
  */
 
 #include "battery_smart.h"
+#include "builtin/assert.h"
 #include "charge_state_v2.h"
 #include "charger.h"
 #include "common.h"
 #include "console.h"
 #include "dptf.h"
+#include "hooks.h"
 #include "host_command.h"
 #include "printf.h"
 #include "util.h"
-#include "hooks.h"
 
 /* Console output macros */
 #define CPUTS(outstr) cputs(CC_CHARGER, outstr)
@@ -96,8 +97,12 @@ void charger_get_params(struct charger_params *chg)
 {
 	int chgnum = 0;
 
-	if (IS_ENABLED(CONFIG_OCPC))
+	if (IS_ENABLED(CONFIG_OCPC)) {
 		chgnum = charge_get_active_chg_chip();
+		/* set to CHARGE_PORT_NONE when no charger connected */
+		if (chgnum < 0)
+			chgnum = 0;
+	}
 
 	memset(chg, 0, sizeof(*chg));
 
@@ -495,8 +500,10 @@ enum ec_error_list charger_discharge_on_ac(int enable)
 	return rv;
 }
 
-enum ec_error_list charger_enable_bypass_mode(int chgnum, int enable)
+enum ec_error_list charger_enable_bypass_mode(int chgnum, bool enable)
 {
+	ASSERT(chgnum >= 0 && chgnum < board_get_charger_chip_count());
+
 	if (!chg_chips[chgnum].drv->enable_bypass_mode)
 		return EC_ERROR_UNIMPLEMENTED;
 	return chg_chips[chgnum].drv->enable_bypass_mode(chgnum, enable);
diff --git a/common/chargesplash.c b/common/chargesplash.c
index 17da9a5c8c..c3d93cc47b 100644
--- a/common/chargesplash.c
+++ b/common/chargesplash.c
@@ -3,9 +3,6 @@
  * found in the LICENSE file.
  */
 
-#include <stdbool.h>
-#include <string.h>
-
 #include "chipset.h"
 #include "common.h"
 #include "console.h"
@@ -18,6 +15,9 @@
 #include "timer.h"
 #include "util.h"
 
+#include <stdbool.h>
+#include <string.h>
+
 #define CPRINTS(format, args...) \
 	cprints(CC_USBCHARGE, "chargesplash: " format, ##args)
 
diff --git a/common/chipset.c b/common/chipset.c
index 55964e4ba9..5a699bfe25 100644
--- a/common/chipset.c
+++ b/common/chipset.c
@@ -134,3 +134,21 @@ enum chipset_shutdown_reason chipset_get_shutdown_reason(void)
 }
 
 #endif /* !CONFIG_AP_RESET_LOG */
+
+#ifdef TEST_BUILD
+uint32_t test_chipset_get_ap_resets_since_ec_boot(void)
+{
+	uint32_t count;
+
+	mutex_lock(&reset_log_mutex);
+	count = ap_resets_since_ec_boot;
+	mutex_unlock(&reset_log_mutex);
+
+	return count;
+}
+
+void test_chipset_corrupt_reset_log_checksum(void)
+{
+	reset_log_checksum = ~reset_log_checksum;
+}
+#endif /* TEST_BUILD */
diff --git a/common/dps.c b/common/dps.c
index c5a8ba4045..b0751acc39 100644
--- a/common/dps.c
+++ b/common/dps.c
@@ -5,16 +5,14 @@
  * Dynamic PDO Selection.
  */
 
-#include <stdint.h>
-
-#include "dps.h"
 #include "atomic.h"
 #include "battery.h"
-#include "common.h"
-#include "console.h"
-#include "charger.h"
 #include "charge_manager.h"
 #include "charge_state.h"
+#include "charger.h"
+#include "common.h"
+#include "console.h"
+#include "dps.h"
 #include "ec_commands.h"
 #include "hooks.h"
 #include "math_util.h"
@@ -24,6 +22,8 @@
 #include "usb_pd.h"
 #include "util.h"
 
+#include <stdint.h>
+
 #define K_MORE_PWR 96
 #define K_LESS_PWR 93
 #define K_SAMPLE 1
@@ -231,7 +231,7 @@ static int get_battery_target_voltage(int *target_mv)
  *
  * @return 0 if error occurs, else battery efficient voltage in mV
  */
-int get_efficient_voltage(void)
+static int get_efficient_voltage(void)
 {
 	int eff_mv = 0;
 	int batt_mv;
diff --git a/common/ec_ec_comm_server.c b/common/ec_ec_comm_server.c
index 6f37004513..174ac54757 100644
--- a/common/ec_ec_comm_server.c
+++ b/common/ec_ec_comm_server.c
@@ -5,16 +5,16 @@
  * EC-EC communication, task and functions for server.
  */
 
-#include "common.h"
 #include "battery.h"
 #include "charge_state_v2.h"
+#include "common.h"
 #include "console.h"
 #include "crc8.h"
 #include "ec_commands.h"
 #include "ec_ec_comm_server.h"
 #include "extpower.h"
-#include "hwtimer.h"
 #include "hooks.h"
+#include "hwtimer.h"
 #include "queue.h"
 #include "queue_policies.h"
 #include "system.h"
@@ -169,8 +169,9 @@ handle_cmd_charger_control(const struct ec_params_charger_control *params,
 			goto out;
 		}
 
-		/* Reset input current to minimum. */
-		charge_set_input_current_limit(CONFIG_CHARGER_INPUT_CURRENT, 0);
+		/* Reset input current to default. */
+		charge_set_input_current_limit(
+			CONFIG_CHARGER_DEFAULT_CURRENT_LIMIT, 0);
 		/* Setup and enable "OTG". */
 		charge_set_output_current_limit(CHARGER_SOLO,
 						-params->max_current,
diff --git a/common/ec_features.c b/common/ec_features.c
index 6778a3b36a..7a69575500 100644
--- a/common/ec_features.c
+++ b/common/ec_features.c
@@ -5,11 +5,12 @@
 
 /* Present Chrome EC device features to the outside world */
 
+#include "board_config.h"
 #include "common.h"
 #include "config.h"
 #include "console.h"
 #include "ec_commands.h"
-#include "board_config.h"
+#include "motion_sense.h"
 
 uint32_t get_feature_flags0(void)
 {
@@ -128,7 +129,10 @@ uint32_t get_feature_flags1(void)
 		| EC_FEATURE_MASK_1(EC_FEATURE_MOTION_SENSE_TIGHT_TIMESTAMPS)
 #endif
 #if defined(CONFIG_LID_ANGLE) && defined(CONFIG_TABLET_MODE)
-		| EC_FEATURE_MASK_1(EC_FEATURE_REFINED_TABLET_MODE_HYSTERESIS)
+		| (sensor_board_is_lid_angle_available() ?
+			   EC_FEATURE_MASK_1(
+				   EC_FEATURE_REFINED_TABLET_MODE_HYSTERESIS) :
+			   0)
 #endif
 #ifdef CONFIG_VBOOT_EFS2
 		| EC_FEATURE_MASK_1(EC_FEATURE_EFS2)
diff --git a/common/espi.c b/common/espi.c
index dfb1f90749..f57e13cf76 100644
--- a/common/espi.c
+++ b/common/espi.c
@@ -6,9 +6,9 @@
 /* eSPI common functionality for Chrome EC */
 
 #include "common.h"
+#include "espi.h"
 #include "gpio.h"
 #include "registers.h"
-#include "espi.h"
 #include "timer.h"
 #include "util.h"
 
diff --git a/common/flash.c b/common/flash.c
index 0a027fd312..4f3f578eed 100644
--- a/common/flash.c
+++ b/common/flash.c
@@ -232,6 +232,13 @@ static int flash_range_ok(int offset, int size_req, int align)
 }
 
 #ifdef CONFIG_MAPPED_STORAGE
+
+/**
+ * A test public variable allowing us to override the base address of
+ * flash_physical_dataptr().
+ */
+test_export_static const char *flash_physical_dataptr_override;
+
 /**
  * Get the physical memory address of a flash offset
  *
@@ -245,6 +252,9 @@ static int flash_range_ok(int offset, int size_req, int align)
  */
 static const char *flash_physical_dataptr(int offset)
 {
+	if (IS_ENABLED(TEST_BUILD) && flash_physical_dataptr_override != NULL) {
+		return flash_physical_dataptr_override + offset;
+	}
 	return (char *)((uintptr_t)CONFIG_MAPPED_STORAGE_BASE + offset);
 }
 
@@ -978,6 +988,8 @@ DECLARE_DEFERRED(flash_erase_deferred);
 /* Console commands */
 
 #ifdef CONFIG_CMD_FLASHINFO
+#define BIT_TO_ON_OFF(value, mask) \
+	((((value) & (mask)) == (mask)) ? "ON" : "OFF")
 static int command_flash_info(int argc, const char **argv)
 {
 	int i, flags;
@@ -1002,36 +1014,35 @@ static int command_flash_info(int argc, const char **argv)
 	ccprintf("Protect: %4d B\n", CONFIG_FLASH_BANK_SIZE);
 #endif
 	flags = crec_flash_get_protect();
-	ccprintf("Flags:  ");
-	if (flags & EC_FLASH_PROTECT_GPIO_ASSERTED)
-		ccputs(" wp_gpio_asserted");
-	if (flags & EC_FLASH_PROTECT_RO_AT_BOOT)
-		ccputs(" ro_at_boot");
-	if (flags & EC_FLASH_PROTECT_ALL_AT_BOOT)
-		ccputs(" all_at_boot");
-	if (flags & EC_FLASH_PROTECT_RO_NOW)
-		ccputs(" ro_now");
-	if (flags & EC_FLASH_PROTECT_ALL_NOW)
-		ccputs(" all_now");
+	ccprintf("Flags:\n");
+	ccprintf("  wp_gpio_asserted: %s\n",
+		 BIT_TO_ON_OFF(flags, EC_FLASH_PROTECT_GPIO_ASSERTED));
+	ccprintf("  ro_at_boot: %s\n",
+		 BIT_TO_ON_OFF(flags, EC_FLASH_PROTECT_RO_AT_BOOT));
+	ccprintf("  all_at_boot: %s\n",
+		 BIT_TO_ON_OFF(flags, EC_FLASH_PROTECT_ALL_AT_BOOT));
+	ccprintf("  ro_now: %s\n",
+		 BIT_TO_ON_OFF(flags, EC_FLASH_PROTECT_RO_NOW));
+	ccprintf("  all_now: %s\n",
+		 BIT_TO_ON_OFF(flags, EC_FLASH_PROTECT_ALL_NOW));
 #ifdef CONFIG_FLASH_PROTECT_RW
-	if (flags & EC_FLASH_PROTECT_RW_AT_BOOT)
-		ccputs(" rw_at_boot");
-	if (flags & EC_FLASH_PROTECT_RW_NOW)
-		ccputs(" rw_now");
+	ccprintf("  rw_at_boot: %s\n",
+		 BIT_TO_ON_OFF(flags, EC_FLASH_PROTECT_RW_AT_BOOT));
+	ccprintf("  rw_now: %s\n",
+		 BIT_TO_ON_OFF(flags, EC_FLASH_PROTECT_RW_NOW));
 #endif
-	if (flags & EC_FLASH_PROTECT_ERROR_STUCK)
-		ccputs(" STUCK");
-	if (flags & EC_FLASH_PROTECT_ERROR_INCONSISTENT)
-		ccputs(" INCONSISTENT");
-	if (flags & EC_FLASH_PROTECT_ERROR_UNKNOWN)
-		ccputs(" UNKNOWN_ERROR");
+	ccprintf("  STUCK: %s\n",
+		 BIT_TO_ON_OFF(flags, EC_FLASH_PROTECT_ERROR_STUCK));
+	ccprintf("  INCONSISTENT: %s\n",
+		 BIT_TO_ON_OFF(flags, EC_FLASH_PROTECT_ERROR_INCONSISTENT));
+	ccprintf("  UNKNOWN_ERROR: %s\n",
+		 BIT_TO_ON_OFF(flags, EC_FLASH_PROTECT_ERROR_UNKNOWN));
 #ifdef CONFIG_ROLLBACK
-	if (flags & EC_FLASH_PROTECT_ROLLBACK_AT_BOOT)
-		ccputs(" rollback_at_boot");
-	if (flags & EC_FLASH_PROTECT_ROLLBACK_NOW)
-		ccputs(" rollback_now");
+	ccprintf("  rollback_at_boot: %s\n",
+		 BIT_TO_ON_OFF(flags, EC_FLASH_PROTECT_ROLLBACK_AT_BOOT));
+	ccprintf("  rollback_now: %s\n",
+		 BIT_TO_ON_OFF(flags, EC_FLASH_PROTECT_ROLLBACK_NOW));
 #endif
-	ccputs("\n");
 
 	ccputs("Protected now:");
 	for (i = 0; i < PHYSICAL_BANKS; i++) {
diff --git a/common/fmap.c b/common/fmap.c
index 469fac0ceb..0fa77c47cd 100644
--- a/common/fmap.c
+++ b/common/fmap.c
@@ -4,13 +4,13 @@
  * found in the LICENSE file.
  */
 
-#include <stddef.h>
-
 #include "common.h"
 #include "cros_version.h"
 #include "rwsig.h"
 #include "util.h"
 
+#include <stddef.h>
+
 /*
  * FMAP structs.
  * See
diff --git a/common/fpsensor/fpsensor_crypto.c b/common/fpsensor/fpsensor_crypto.c
index 257042197e..aac50bb297 100644
--- a/common/fpsensor/fpsensor_crypto.c
+++ b/common/fpsensor/fpsensor_crypto.c
@@ -2,16 +2,16 @@
  * Use of this source code is governed by a BSD-style license that can be
  * found in the LICENSE file.
  */
-#include <stdbool.h>
-
-#include "aes.h"
 #include "aes-gcm.h"
+#include "aes.h"
 #include "cryptoc/util.h"
 #include "fpsensor_crypto.h"
 #include "fpsensor_private.h"
 #include "fpsensor_state.h"
 #include "rollback.h"
 
+#include <stdbool.h>
+
 #if !defined(CONFIG_AES) || !defined(CONFIG_AES_GCM) || \
 	!defined(CONFIG_ROLLBACK_SECRET_SIZE)
 #error "fpsensor requires AES, AES_GCM and ROLLBACK_SECRET_SIZE"
diff --git a/common/gesture.c b/common/gesture.c
index 99d054783d..48e6af3571 100644
--- a/common/gesture.c
+++ b/common/gesture.c
@@ -8,8 +8,8 @@
 #include "accelgyro.h"
 #include "common.h"
 #include "console.h"
-#include "hooks.h"
 #include "gesture.h"
+#include "hooks.h"
 #include "lid_switch.h"
 #include "lightbar.h"
 #include "motion_sense.h"
diff --git a/common/gpio_commands.c b/common/gpio_commands.c
index d6257ea61d..06e0203090 100644
--- a/common/gpio_commands.c
+++ b/common/gpio_commands.c
@@ -91,7 +91,10 @@ __maybe_unused static const struct gpio_flag_description gpio_descriptions[] = {
 	{ GPIO_INPUT, "I" },	    { GPIO_OUTPUT, "O" },
 	{ GPIO_LOW, "L" },	    { GPIO_HIGH, "H" },
 	{ GPIO_OPEN_DRAIN, "ODR" }, { GPIO_PULL_UP, "PU" },
-	{ GPIO_PULL_DOWN, "PD" },   { GPIO_SEL_1P8V, "1P8" },
+	{ GPIO_PULL_DOWN, "PD" },
+#ifdef GPIO_SEL_1P8V
+	{ GPIO_SEL_1P8V, "1P8" },
+#endif
 #ifndef CONFIG_ZEPHYR
 	{ GPIO_ANALOG, "A" },	    { GPIO_ALTERNATE, "ALT" },
 	{ GPIO_LOCKED, "LCK" }
diff --git a/common/gyro_cal.c b/common/gyro_cal.c
index 8996b85757..e2cb8d55db 100644
--- a/common/gyro_cal.c
+++ b/common/gyro_cal.c
@@ -5,6 +5,7 @@
 
 #include "gyro_cal.h"
 #include "string.h"
+
 #include <stdbool.h>
 
 /*
diff --git a/common/host_command_pd.c b/common/host_command_pd.c
index 23e2710695..e7dbbc2b5f 100644
--- a/common/host_command_pd.c
+++ b/common/host_command_pd.c
@@ -126,10 +126,7 @@ static void pd_check_chg_status(struct ec_response_pd_status *pd_status)
 #endif
 
 	/* Set input current limit */
-	rv = charge_set_input_current_limit(
-		MAX(pd_status->curr_lim_ma, CONFIG_CHARGER_INPUT_CURRENT), 0);
-	if (rv < 0)
-		CPRINTS("Failed to set input curr limit from PD MCU");
+	board_set_charge_limit(pd_status->curr_lim_ma, 0);
 }
 #endif /* CONFIG_HOSTCMD_PD_CHG_CTRL */
 #endif /* CONFIG_HOSTCMD_PD */
diff --git a/common/i2c_controller.c b/common/i2c_controller.c
index b51f726369..96809ee84c 100644
--- a/common/i2c_controller.c
+++ b/common/i2c_controller.c
@@ -7,12 +7,12 @@
 
 #include "battery.h"
 #include "builtin/assert.h"
-#include "clock.h"
 #include "charge_state.h"
+#include "clock.h"
 #include "console.h"
 #include "crc8.h"
-#include "host_command.h"
 #include "gpio.h"
+#include "host_command.h"
 #include "i2c.h"
 #include "i2c_bitbang.h"
 #include "i2c_private.h"
@@ -22,12 +22,13 @@
 #include "usb_pd.h"
 #include "usb_pd_tcpm.h"
 #include "util.h"
-#include "watchdog.h"
 #include "virtual_battery.h"
+#include "watchdog.h"
 
 #ifdef CONFIG_ZEPHYR
-#include <zephyr/drivers/i2c.h>
 #include "i2c/i2c.h"
+
+#include <zephyr/drivers/i2c.h>
 #endif /* CONFIG_ZEPHYR */
 
 #define CPUTS(outstr) cputs(CC_I2C, outstr)
@@ -833,7 +834,7 @@ int i2c_write_block(const int port, const uint16_t addr_flags, int offset,
 		if (rv)
 			continue;
 
-		if (I2C_USE_PEC(addr_flags)) {
+		if (IS_ENABLED(CONFIG_SMBUS_PEC) && I2C_USE_PEC(addr_flags)) {
 			rv = i2c_xfer_unlocked(port, addr_flags, data, len,
 					       NULL, 0, 0);
 			if (rv)
diff --git a/common/i2c_controller_cros_ec.c b/common/i2c_controller_cros_ec.c
index aafeb504ce..d68f67c60e 100644
--- a/common/i2c_controller_cros_ec.c
+++ b/common/i2c_controller_cros_ec.c
@@ -4,16 +4,15 @@
  */
 
 #include "builtin/assert.h"
-#include "system.h"
+#include "console.h"
 #include "gpio.h"
-
+#include "i2c.h"
 #include "i2c_bitbang.h"
 #include "i2c_private.h"
-#include "i2c.h"
-#include "console.h"
-#include "watchdog.h"
 #include "printf.h"
+#include "system.h"
 #include "util.h"
+#include "watchdog.h"
 
 /* This source file contains I2C controller code that is used only in legacy
  * (CrOS EC) builds.
diff --git a/common/i2c_hid_touchpad.c b/common/i2c_hid_touchpad.c
index 02261d8fa4..2664dabe3e 100644
--- a/common/i2c_hid_touchpad.c
+++ b/common/i2c_hid_touchpad.c
@@ -3,10 +3,9 @@
  * found in the LICENSE file.
  */
 
-#include "i2c_hid_touchpad.h"
-
 #include "console.h"
 #include "hwtimer.h"
+#include "i2c_hid_touchpad.h"
 #include "util.h"
 
 /* 2 bytes for length + 1 byte for report ID */
diff --git a/common/i2c_trace.c b/common/i2c_trace.c
index c7207698b7..8532766ea8 100644
--- a/common/i2c_trace.c
+++ b/common/i2c_trace.c
@@ -6,8 +6,8 @@
 #include "common.h"
 #include "console.h"
 #include "i2c.h"
-#include "stddef.h"
 #include "stdbool.h"
+#include "stddef.h"
 #include "util.h"
 
 #define CPUTS(outstr) cputs(CC_I2C, outstr)
@@ -67,7 +67,7 @@ static int command_i2ctrace_list(void)
 			i2c_port = get_i2c_port(trace_entries[i].port);
 			ccprintf("%-2zd %d %-8s 0x%X", i, trace_entries[i].port,
 #ifndef CONFIG_ZEPHYR
-				 i2c_port->name,
+				 i2c_port ? i2c_port->name : "invalid",
 #else
 				 "",
 #endif /* CONFIG_ZEPHYR */
diff --git a/common/init_rom.c b/common/init_rom.c
index 102c3a00ae..ad9e80f423 100644
--- a/common/init_rom.c
+++ b/common/init_rom.c
@@ -7,8 +7,8 @@
 
 #include "builtin/assert.h"
 #include "common.h"
-#include "init_rom.h"
 #include "flash.h"
+#include "init_rom.h"
 #include "stdbool.h"
 #include "stddef.h"
 
diff --git a/common/kasa.c b/common/kasa.c
index 6b974dc574..0ab716c729 100644
--- a/common/kasa.c
+++ b/common/kasa.c
@@ -6,6 +6,7 @@
 #include "common.h"
 #include "kasa.h"
 #include "mat44.h"
+
 #include <string.h>
 
 void kasa_reset(struct kasa_fit *kasa)
diff --git a/common/keyboard_8042.c b/common/keyboard_8042.c
index e0371ddd5c..6e2b14719f 100644
--- a/common/keyboard_8042.c
+++ b/common/keyboard_8042.c
@@ -5,8 +5,9 @@
  * 8042 keyboard protocol
  */
 
-#include "button.h"
+#include "atkbd_protocol.h"
 #include "builtin/assert.h"
+#include "button.h"
 #include "chipset.h"
 #include "common.h"
 #include "console.h"
@@ -14,7 +15,6 @@
 #include "hooks.h"
 #include "host_command.h"
 #include "i8042_protocol.h"
-#include "atkbd_protocol.h"
 #include "keyboard_8042_sharedlib.h"
 #include "keyboard_config.h"
 #include "keyboard_protocol.h"
@@ -78,6 +78,15 @@ enum scancode_set_list {
 #define KB_TO_HOST_RETRIES 3
 
 /*
+ * Timeout for SETLEDS command. Kernel is supposed to send the second byte
+ * within this period. When timeout occurs, the second byte is received as
+ * 'Unsupported AT keyboard command 0x00' (or 0x04). You can evaluate your
+ * timeout is too long or too short by calculating the duration between 'KB
+ * SETLEDS' and 'Unsupported AT...'.
+ */
+#define SETLEDS_TIMEOUT (30 * MSEC)
+
+/*
  * Mutex to control write access to the to-host buffer head.  Don't need to
  * mutex the tail because reads are only done in one place.
  */
@@ -87,6 +96,7 @@ static mutex_t to_host_mutex;
 enum {
 	CHAN_KBD = 0,
 	CHAN_AUX,
+	CHAN_CMD,
 };
 struct data_byte {
 	uint8_t chan;
@@ -94,6 +104,7 @@ struct data_byte {
 };
 
 static struct queue const to_host = QUEUE_NULL(16, struct data_byte);
+static struct queue const to_host_cmd = QUEUE_NULL(16, struct data_byte);
 
 /* Queue command/data from the host */
 enum {
@@ -162,6 +173,7 @@ static int typematic_inter_delay;
 static int typematic_len; /* length of typematic_scan_code */
 static uint8_t typematic_scan_code[MAX_SCAN_CODE_LEN];
 static timestamp_t typematic_deadline;
+static timestamp_t setleds_deadline;
 
 #define KB_SYSJUMP_TAG 0x4b42 /* "KB" */
 #define KB_HOOK_VERSION 2
@@ -182,18 +194,16 @@ struct kblog_t {
 	/*
 	 * Type:
 	 *
-	 * s = byte enqueued to send to host
 	 * a = aux byte enqueued to send to host
-	 * t = to-host queue tail pointer before type='s' bytes enqueued
-	 *
-	 * d = data byte from host
 	 * c = command byte from host
-	 *
-	 * k = to-host queue head pointer before byte dequeued
-	 * K = byte actually sent to host via LPC
-	 * A = byte actually sent to host via LPC as AUX
-	 *
+	 * d = data byte from host
+	 * r = typematic
+	 * s = byte enqueued to send to host
+	 * t = to-host queue tail pointer before type='s' bytes enqueued
+	 * u = byte enqueued to send to host with priority
 	 * x = to_host queue was cleared
+	 * A = byte actually sent to host via LPC as AUX
+	 * K = byte actually sent to host via LPC
 	 *
 	 * The to-host head and tail pointers are logged pre-wrapping to the
 	 * queue size.  This means that they continually increment as units
@@ -265,7 +275,7 @@ static void aux_enable_irq(int enable)
  * host cannot read the previous byte away in time.
  *
  * @param len		Number of bytes to send to the host
- * @param to_host	Data to send
+ * @param bytes		Data to send
  * @param chan		Channel to send data on
  */
 static void i8042_send_to_host(int len, const uint8_t *bytes, uint8_t chan,
@@ -281,15 +291,29 @@ static void i8042_send_to_host(int len, const uint8_t *bytes, uint8_t chan,
 		for (i = 0; i < len; i++)
 			kblog_put('r', bytes[i]);
 	} else {
-		for (i = 0; i < len; i++)
-			kblog_put(chan == CHAN_AUX ? 'a' : 's', bytes[i]);
+		struct queue const *queue = &to_host;
+
+		if (chan == CHAN_CMD)
+			queue = &to_host_cmd;
+
+		for (i = 0; i < len; i++) {
+			char type;
+
+			if (chan == CHAN_AUX)
+				type = 'a';
+			else if (chan == CHAN_CMD)
+				type = 'u';
+			else
+				type = 's';
+			kblog_put(type, bytes[i]);
+		}
 
-		if (queue_space(&to_host) >= len) {
-			kblog_put('t', to_host.state->tail);
+		if (queue_space(queue) >= len) {
+			kblog_put('t', queue->state->tail);
 			for (i = 0; i < len; i++) {
 				data.chan = chan;
 				data.byte = bytes[i];
-				queue_add_unit(&to_host, &data);
+				queue_add_unit(queue, &data);
 			}
 		}
 	}
@@ -417,6 +441,7 @@ void keyboard_clear_buffer(void)
 	mutex_lock(&to_host_mutex);
 	kblog_put('x', queue_count(&to_host));
 	queue_init(&to_host);
+	queue_init(&to_host_cmd);
 	mutex_unlock(&to_host_mutex);
 	lpc_keyboard_clear_buffer();
 }
@@ -661,6 +686,8 @@ static int handle_keyboard_data(uint8_t data, uint8_t *output)
 			/* Chrome OS doesn't have keyboard LEDs, so ignore */
 			output[out_len++] = ATKBD_RET_ACK;
 			data_port_state = STATE_ATKBD_SETLEDS;
+			setleds_deadline.val = get_time().val + SETLEDS_TIMEOUT;
+			CPRINTS5("KB SETLEDS");
 			break;
 
 		case ATKBD_CMD_EX_SETLEDS:
@@ -868,20 +895,23 @@ static void i8042_handle_from_host(void)
 	struct host_byte h;
 	int ret_len;
 	uint8_t output[MAX_SCAN_CODE_LEN];
-	uint8_t chan = CHAN_KBD;
+	uint8_t chan;
 
 	while (queue_remove_unit(&from_host, &h)) {
 		if (h.type == HOST_COMMAND) {
 			ret_len = handle_keyboard_command(h.byte, output);
+			chan = CHAN_KBD;
 		} else {
 			CPRINTS5("KB recv data: 0x%02x", h.byte);
 			kblog_put('d', h.byte);
 
 			if (IS_ENABLED(CONFIG_8042_AUX) &&
-			    handle_mouse_data(h.byte, output, &ret_len))
+			    handle_mouse_data(h.byte, output, &ret_len)) {
 				chan = CHAN_AUX;
-			else
+			} else {
 				ret_len = handle_keyboard_data(h.byte, output);
+				chan = CHAN_CMD;
+			}
 		}
 
 		i8042_send_to_host(ret_len, output, chan, 0);
@@ -925,10 +955,14 @@ void keyboard_protocol_task(void *u)
 			i8042_handle_from_host();
 
 			/* Check if we have data to send to host */
-			if (queue_is_empty(&to_host))
+			if (queue_is_empty(&to_host) &&
+			    queue_is_empty(&to_host_cmd))
 				break;
 
-			/* Handle data waiting for host */
+			/*
+			 * Check if the output buffer is full. We can't proceed
+			 * until the host read the data.
+			 */
 			if (lpc_keyboard_has_char()) {
 				/* If interrupts disabled, nothing we can do */
 				if (!i8042_keyboard_irq_enabled &&
@@ -946,26 +980,53 @@ void keyboard_protocol_task(void *u)
 				 * data?  Send it another interrupt in case it
 				 * somehow missed the first one.
 				 */
-				CPRINTS("KB extra IRQ");
+				CPRINTS("KB host not responding");
 				lpc_keyboard_resume_irq();
 				retries = 0;
 				break;
 			}
 
+			/*
+			 * We know DBBOUT is empty but we need act quickly as
+			 * the host might be sending a byte to DBBIN.
+			 *
+			 * So be cautious if you're adding any code below up to
+			 * lpc_keyboard_put_char since that'll increase the race
+			 * condition. For example, you don't want to add CPRINTS
+			 * or kblog_put.
+			 *
+			 * We should claim OBF=1 atomically to prevent the host
+			 * from writing to DBBIN (i.e. set-ibf-if-not-obf). It's
+			 * not possible for NPCX because NPCX's HIKMST-IBF is
+			 * read-only.
+			 */
+
 			/* Get a char from buffer. */
-			kblog_put('k', to_host.state->head);
-			queue_remove_unit(&to_host, &entry);
+			if (queue_count(&to_host_cmd)) {
+				queue_remove_unit(&to_host_cmd, &entry);
+			} else if (data_port_state == STATE_ATKBD_SETLEDS) {
+				/* to_host_cmd is empty but in SETLEDS */
+				if (!timestamp_expired(setleds_deadline, &t))
+					/* Let's wait for the 2nd byte. */
+					break;
+				/* Didn't receive 2nd byte. Go back to CMD. */
+				CPRINTS("KB SETLEDS timeout");
+				data_port_state = STATE_ATKBD_CMD;
+			} else {
+				/* to_host isn't empty && not in SETLEDS */
+				queue_remove_unit(&to_host, &entry);
+			}
 
 			/* Write to host. */
 			if (entry.chan == CHAN_AUX &&
 			    IS_ENABLED(CONFIG_8042_AUX)) {
-				kblog_put('A', entry.byte);
 				lpc_aux_put_char(entry.byte,
 						 i8042_aux_irq_enabled);
+				kblog_put('A', entry.byte);
 			} else {
-				kblog_put('K', entry.byte);
 				lpc_keyboard_put_char(
 					entry.byte, i8042_keyboard_irq_enabled);
+				kblog_put('K', entry.byte);
 			}
 			retries = 0;
 		}
diff --git a/common/keyboard_8042_sharedlib.c b/common/keyboard_8042_sharedlib.c
index 3e98c977b7..810f9873b3 100644
--- a/common/keyboard_8042_sharedlib.c
+++ b/common/keyboard_8042_sharedlib.c
@@ -5,8 +5,6 @@
  * Objects which can be shared between RO and RW for 8042 keyboard protocol.
  */
 
-#include <stddef.h>
-
 #include "builtin/assert.h"
 #include "button.h"
 #include "keyboard_8042_sharedlib.h"
@@ -15,6 +13,8 @@
 #include "libsharedobjs.h"
 #include "util.h"
 
+#include <stddef.h>
+
 #ifndef CONFIG_KEYBOARD_CUSTOMIZATION
 /* The standard Chrome OS keyboard matrix table in scan code set 2. */
 static uint16_t scancode_set2[KEYBOARD_COLS_MAX][KEYBOARD_ROWS] = {
diff --git a/common/keyboard_scan.c b/common/keyboard_scan.c
index 6c3756170b..617ae3fc02 100644
--- a/common/keyboard_scan.c
+++ b/common/keyboard_scan.c
@@ -913,6 +913,7 @@ void keyboard_scan_task(void *u)
 	uint32_t local_disable_scanning = 0;
 
 	print_state(debounced_state, "init state");
+	poll_deadline.val = 0;
 
 	keyboard_raw_task_start();
 
@@ -1078,6 +1079,10 @@ int keyboard_factory_test_scan(void)
 	keyboard_scan_enable(0, KB_SCAN_DISABLE_LID_CLOSED);
 	flags = gpio_get_default_flags(GPIO_KBD_KSO2);
 
+	if (IS_ENABLED(CONFIG_ZEPHYR))
+		/* set all KSI/KSO pins to GPIO_ALT_FUNC_NONE */
+		keybaord_raw_config_alt(0);
+
 	/* Set all of KSO/KSI pins to internal pull-up and input */
 	for (i = 0; i < keyboard_factory_scan_pins_used; i++) {
 		if (keyboard_factory_scan_pins[i][0] < 0)
@@ -1086,7 +1091,9 @@ int keyboard_factory_test_scan(void)
 		port = keyboard_factory_scan_pins[i][0];
 		id = keyboard_factory_scan_pins[i][1];
 
-		gpio_set_alternate_function(port, 1 << id, GPIO_ALT_FUNC_NONE);
+		if (!IS_ENABLED(CONFIG_ZEPHYR))
+			gpio_set_alternate_function(port, 1 << id,
+						    GPIO_ALT_FUNC_NONE);
 		gpio_set_flags_by_mask(port, 1 << id,
 				       GPIO_INPUT | GPIO_PULL_UP);
 	}
@@ -1119,7 +1126,10 @@ int keyboard_factory_test_scan(void)
 				       GPIO_INPUT | GPIO_PULL_UP);
 	}
 done:
-	gpio_config_module(MODULE_KEYBOARD_SCAN, 1);
+	if (IS_ENABLED(CONFIG_ZEPHYR))
+		keybaord_raw_config_alt(1);
+	else
+		gpio_config_module(MODULE_KEYBOARD_SCAN, 1);
 	gpio_set_flags(GPIO_KBD_KSO2, flags);
 	keyboard_scan_enable(1, KB_SCAN_DISABLE_LID_CLOSED);
 
diff --git a/common/keyboard_vivaldi.c b/common/keyboard_vivaldi.c
index 11f5b708e6..44bbff2c11 100644
--- a/common/keyboard_vivaldi.c
+++ b/common/keyboard_vivaldi.c
@@ -6,13 +6,14 @@
 /* Vivali Keyboard code for Chrome EC */
 
 #include "builtin/assert.h"
-#include "keyboard_8042_sharedlib.h"
-#include "keyboard_scan.h"
 #include "ec_commands.h"
 #include "gpio.h"
+#include "keyboard_8042_sharedlib.h"
+#include "keyboard_scan.h"
+
+#include <hooks.h>
 #include <host_command.h>
 #include <util.h>
-#include <hooks.h>
 
 /* Console output macros */
 #define CPUTS(outstr) cputs(CC_KEYBOARD, outstr)
diff --git a/common/led_common.c b/common/led_common.c
index 2c73ba95e6..cabaf22e10 100644
--- a/common/led_common.c
+++ b/common/led_common.c
@@ -47,6 +47,10 @@ int led_auto_control_is_enabled(enum ec_led_id led_id)
 	return (led_auto_control_flags & LED_AUTO_CONTROL_FLAG(led_id)) != 0;
 }
 
+/* Empty functions cannot be verified in testing to not have had any
+ * side-effects, remove from coverage.
+ * LCOV_EXCL_START
+ */
 __attribute__((weak)) void board_led_auto_control(void)
 {
 	/*
@@ -56,6 +60,7 @@ __attribute__((weak)) void board_led_auto_control(void)
 	 * is changed.
 	 */
 }
+/* LCOV_EXCL_STOP */
 
 static enum ec_status led_command_control(struct host_cmd_handler_args *args)
 {
@@ -91,6 +96,7 @@ static enum ec_status led_command_control(struct host_cmd_handler_args *args)
 }
 DECLARE_HOST_COMMAND(EC_CMD_LED_CONTROL, led_command_control, EC_VER_MASK(1));
 
+#ifndef CONFIG_ZEPHYR
 __attribute__((weak)) void led_control(enum ec_led_id led_id,
 				       enum ec_led_state state)
 {
@@ -99,3 +105,4 @@ __attribute__((weak)) void led_control(enum ec_led_id led_id,
 	 * LED. Boards can provide their own implementation.
 	 */
 }
+#endif
diff --git a/common/led_policy_std.c b/common/led_policy_std.c
index fe8570df87..b9d95af639 100644
--- a/common/led_policy_std.c
+++ b/common/led_policy_std.c
@@ -6,14 +6,14 @@
  * This assumes a red/green battery led and a single power led.
  */
 
-#include "gpio.h"
-#include "hooks.h"
 #include "battery.h"
 #include "charge_state.h"
 #include "chipset.h"
+#include "gpio.h"
+#include "hooks.h"
 #include "led_common.h"
-#include "util.h"
 #include "lid_switch.h"
+#include "util.h"
 
 #ifdef CONFIG_LED_BAT_ACTIVE_LOW
 #define BAT_LED_ON 0
diff --git a/common/lightbar.c b/common/lightbar.c
index 8a6a874b34..a705a2cbed 100644
--- a/common/lightbar.c
+++ b/common/lightbar.c
@@ -17,8 +17,8 @@
 #include "hooks.h"
 #include "host_command.h"
 #include "lb_common.h"
-#include "lightbar.h"
 #include "lid_switch.h"
+#include "lightbar.h"
 #include "motion_sense.h"
 #include "pwm.h"
 #include "system.h"
diff --git a/common/mag_cal.c b/common/mag_cal.c
index a9ce95798c..2ad89c48df 100644
--- a/common/mag_cal.c
+++ b/common/mag_cal.c
@@ -8,7 +8,6 @@
 #include "mag_cal.h"
 #include "mat33.h"
 #include "mat44.h"
-
 #include "math.h"
 #include "math_util.h"
 #include "util.h"
diff --git a/common/mkbp_event.c b/common/mkbp_event.c
index e602f81baf..eb57a11656 100644
--- a/common/mkbp_event.c
+++ b/common/mkbp_event.c
@@ -11,10 +11,10 @@
 #include "host_command.h"
 #include "host_command_heci.h"
 #include "hwtimer.h"
-#include "timer.h"
 #include "link_defs.h"
 #include "mkbp_event.h"
 #include "power.h"
+#include "timer.h"
 #include "util.h"
 
 #define CPUTS(outstr) cputs(CC_COMMAND, outstr)
@@ -586,5 +586,12 @@ void mkbp_event_clear_all(void)
 
 	/* Reset the interrupt line */
 	mkbp_set_host_active(0, NULL);
+#ifdef CONFIG_MKBP_EVENT_WAKEUP_MASK
+	mkbp_event_wake_mask = CONFIG_MKBP_EVENT_WAKEUP_MASK;
+#endif /* CONFIG_MKBP_EVENT_WAKEUP_MASK */
+
+#ifdef CONFIG_MKBP_HOST_EVENT_WAKEUP_MASK
+	mkbp_host_event_wake_mask = CONFIG_MKBP_HOST_EVENT_WAKEUP_MASK;
+#endif /* CONFIG_MKBP_HOST_EVENT_WAKEUP_MASK */
 }
 #endif
diff --git a/common/mock/charge_manager_mock.c b/common/mock/charge_manager_mock.c
index c800b9b9b1..745b37efa7 100644
--- a/common/mock/charge_manager_mock.c
+++ b/common/mock/charge_manager_mock.c
@@ -8,12 +8,12 @@
  * @brief Mock charge_manager
  */
 
-#include <stdlib.h>
-
 #include "charge_manager.h"
 #include "common.h"
 #include "mock/charge_manager_mock.h"
 
+#include <stdlib.h>
+
 #ifndef TEST_BUILD
 #error "Mocks should only be in the test build."
 #endif
diff --git a/common/mock/dp_alt_mode_mock.c b/common/mock/dp_alt_mode_mock.c
index e261415aaa..ed769ed4b5 100644
--- a/common/mock/dp_alt_mode_mock.c
+++ b/common/mock/dp_alt_mode_mock.c
@@ -10,8 +10,8 @@
  */
 
 #include "console.h"
-#include "usb_dp_alt_mode.h"
 #include "mock/dp_alt_mode_mock.h"
+#include "usb_dp_alt_mode.h"
 
 #ifndef TEST_BUILD
 #error "Mocks should only be in the test build."
diff --git a/common/mock/fp_sensor_mock.c b/common/mock/fp_sensor_mock.c
index 4db25a821c..57a4f42ae9 100644
--- a/common/mock/fp_sensor_mock.c
+++ b/common/mock/fp_sensor_mock.c
@@ -8,12 +8,12 @@
  * @brief Mock fpsensor private driver
  */
 
-#include <stdlib.h>
-
 #include "common.h"
 #include "fpsensor.h"
 #include "mock/fp_sensor_mock.h"
 
+#include <stdlib.h>
+
 #ifndef TEST_BUILD
 #error "Mocks should only be in the test build."
 #endif
diff --git a/common/mock/fpsensor_crypto_mock.c b/common/mock/fpsensor_crypto_mock.c
index f2cc425c8f..334f98300a 100644
--- a/common/mock/fpsensor_crypto_mock.c
+++ b/common/mock/fpsensor_crypto_mock.c
@@ -7,16 +7,17 @@
  * @file fpsensor_crypto_mock.c
  * @brief Mock fpsensor_crypto library
  */
-#include "sha256.h"
-#include <stdio.h>
-#include <stdint.h>
-#include <string.h>
 #include "assert.h"
 #include "compile_time_macros.h"
 #include "console.h"
 #include "ec_commands.h"
 #include "fpsensor_private.h"
 #include "mock/fpsensor_crypto_mock.h"
+#include "sha256.h"
+
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
 
 #ifndef TEST_BUILD
 #error "Mocks should only be in the test build."
diff --git a/common/mock/fpsensor_state_mock.c b/common/mock/fpsensor_state_mock.c
index cbeb29ae3c..d71285c1de 100644
--- a/common/mock/fpsensor_state_mock.c
+++ b/common/mock/fpsensor_state_mock.c
@@ -3,14 +3,14 @@
  * found in the LICENSE file.
  */
 
-#include <stddef.h>
-#include <string.h>
-
 #include "common.h"
 #include "driver/fingerprint/fpsensor.h"
 #include "ec_commands.h"
 #include "test_util.h"
 
+#include <stddef.h>
+#include <string.h>
+
 #ifndef TEST_BUILD
 #error "Mocks should only be in the test build."
 #endif
diff --git a/common/mock/mkbp_events_mock.c b/common/mock/mkbp_events_mock.c
index 3f4d76b659..559cb1494d 100644
--- a/common/mock/mkbp_events_mock.c
+++ b/common/mock/mkbp_events_mock.c
@@ -8,11 +8,11 @@
  * @brief Mock event handling for MKBP keyboard protocol
  */
 
-#include <stdint.h>
-
 #include "common.h"
 #include "mock/mkbp_events_mock.h"
 
+#include <stdint.h>
+
 #ifndef TEST_BUILD
 #error "Mocks should only be in the test build."
 #endif
diff --git a/common/mock/rollback_latest_mock.c b/common/mock/rollback_latest_mock.c
index ab62645a1c..b3433246d7 100644
--- a/common/mock/rollback_latest_mock.c
+++ b/common/mock/rollback_latest_mock.c
@@ -8,13 +8,13 @@
  * @brief Mock rollback block library
  */
 
-#include <string.h>
-
 #include "common.h"
 #include "compile_time_macros.h"
-#include "util.h"
 #include "mock/rollback_latest_mock.h"
 #include "rollback_private.h"
+#include "util.h"
+
+#include <string.h>
 
 #ifndef TEST_BUILD
 #error "Mocks should only be in the test build."
diff --git a/common/mock/rollback_mock.c b/common/mock/rollback_mock.c
index e68616df53..9395248a61 100644
--- a/common/mock/rollback_mock.c
+++ b/common/mock/rollback_mock.c
@@ -8,13 +8,13 @@
  * @brief Mock rollback block library
  */
 
-#include <stdint.h>
-#include <string.h>
-
 #include "common.h"
 #include "compile_time_macros.h"
-#include "util.h"
 #include "mock/rollback_mock.h"
+#include "util.h"
+
+#include <stdint.h>
+#include <string.h>
 
 #ifndef TEST_BUILD
 #error "Mocks should only be in the test build."
diff --git a/common/mock/usb_mux_mock.c b/common/mock/usb_mux_mock.c
index 815cdf777c..b612f47d4f 100644
--- a/common/mock/usb_mux_mock.c
+++ b/common/mock/usb_mux_mock.c
@@ -6,9 +6,9 @@
 
 #include "common.h"
 #include "console.h"
-#include "usb_mux.h"
-#include "mock/usb_mux_mock.h"
 #include "memory.h"
+#include "mock/usb_mux_mock.h"
+#include "usb_mux.h"
 
 #ifndef CONFIG_COMMON_RUNTIME
 #define cprints(format, args...)
diff --git a/common/mock/usb_pd_dpm_mock.c b/common/mock/usb_pd_dpm_mock.c
index 158c53c650..66b7ccfc2b 100644
--- a/common/mock/usb_pd_dpm_mock.c
+++ b/common/mock/usb_pd_dpm_mock.c
@@ -8,9 +8,9 @@
  * Refer to USB PD 3.0 spec, version 2.0, sections 8.2 and 8.3
  */
 
-#include "usb_pd.h"
-#include "mock/usb_pd_dpm_mock.h"
 #include "memory.h"
+#include "mock/usb_pd_dpm_mock.h"
+#include "usb_pd.h"
 #include "usb_pd_tcpm.h"
 
 #ifndef TEST_BUILD
@@ -49,7 +49,7 @@ void dpm_set_mode_exit_request(int port)
 {
 }
 
-void dpm_run(int port)
+void dpm_run(int port, int evt, int en)
 {
 }
 
@@ -95,3 +95,7 @@ int dpm_get_status_msg(int port, uint8_t *msg, uint32_t *len)
 void dpm_handle_alert(int port, uint32_t ado)
 {
 }
+
+void dpm_set_pe_ready(int port, bool enable)
+{
+}
diff --git a/common/mock/usb_pe_sm_mock.c b/common/mock/usb_pe_sm_mock.c
index 24861a73f9..38df938c46 100644
--- a/common/mock/usb_pe_sm_mock.c
+++ b/common/mock/usb_pe_sm_mock.c
@@ -7,11 +7,11 @@
 
 #include "common.h"
 #include "console.h"
-#include "usb_pd.h"
-#include "usb_pe_sm.h"
-#include "mock/usb_pe_sm_mock.h"
 #include "memory.h"
+#include "mock/usb_pe_sm_mock.h"
+#include "usb_pd.h"
 #include "usb_pd_tcpm.h"
+#include "usb_pe_sm.h"
 
 #ifndef CONFIG_COMMON_RUNTIME
 #define cprints(format, args...)
diff --git a/common/mock/usb_prl_mock.c b/common/mock/usb_prl_mock.c
index df61cfaf95..769cd6073c 100644
--- a/common/mock/usb_prl_mock.c
+++ b/common/mock/usb_prl_mock.c
@@ -4,16 +4,17 @@
  *
  * Mock Protocol Layer module.
  */
-#include <string.h>
 #include "common.h"
-#include "usb_emsg.h"
-#include "usb_pe_sm.h"
-#include "usb_prl_sm.h"
 #include "mock/usb_prl_mock.h"
 #include "task.h"
 #include "test_util.h"
 #include "timer.h"
+#include "usb_emsg.h"
 #include "usb_pd_tcpm.h"
+#include "usb_pe_sm.h"
+#include "usb_prl_sm.h"
+
+#include <string.h>
 
 #ifndef TEST_BUILD
 #error "Mocks should only be in the test build."
diff --git a/common/mock/usb_tc_sm_mock.c b/common/mock/usb_tc_sm_mock.c
index 5badc6eba6..6adf6b3246 100644
--- a/common/mock/usb_tc_sm_mock.c
+++ b/common/mock/usb_tc_sm_mock.c
@@ -8,9 +8,9 @@
 #include "common.h"
 #include "console.h"
 #include "ec_commands.h"
-#include "usb_tc_sm.h"
-#include "mock/usb_tc_sm_mock.h"
 #include "memory.h"
+#include "mock/usb_tc_sm_mock.h"
+#include "usb_tc_sm.h"
 
 #ifndef CONFIG_COMMON_RUNTIME
 #define cprints(format, args...)
diff --git a/common/motion_lid.c b/common/motion_lid.c
index d8445bb3d4..878d471ed4 100644
--- a/common/motion_lid.c
+++ b/common/motion_lid.c
@@ -5,8 +5,8 @@
 
 /* Motion sense module to read from various motion sensors. */
 
-#include "acpi.h"
 #include "accelgyro.h"
+#include "acpi.h"
 #include "chipset.h"
 #include "common.h"
 #include "console.h"
@@ -20,8 +20,8 @@
 #include "motion_sense.h"
 #include "power.h"
 #include "tablet_mode.h"
-#include "timer.h"
 #include "task.h"
+#include "timer.h"
 #include "util.h"
 
 /* Console output macros */
diff --git a/common/motion_sense.c b/common/motion_sense.c
index c08909d24e..0b72a69780 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -20,17 +20,17 @@
 #include "lightbar.h"
 #include "math_util.h"
 #include "mkbp_event.h"
-#include "motion_sense.h"
-#include "motion_sense_fifo.h"
 #include "motion_lid.h"
 #include "motion_orientation.h"
+#include "motion_sense.h"
+#include "motion_sense_fifo.h"
 #include "online_calibration.h"
-#include "printf.h"
 #include "power.h"
+#include "printf.h"
 #include "queue.h"
 #include "tablet_mode.h"
-#include "timer.h"
 #include "task.h"
+#include "timer.h"
 #include "util.h"
 
 /* Console output macros */
@@ -81,6 +81,13 @@ static int init_sensor_mutex(const struct device *dev)
 SYS_INIT(init_sensor_mutex, POST_KERNEL, 50);
 #endif /* CONFIG_ZEPHYR */
 
+#ifdef CONFIG_LID_ANGLE
+__attribute__((weak)) int sensor_board_is_lid_angle_available(void)
+{
+	return 1;
+}
+#endif
+
 static inline int
 motion_sensor_in_forced_mode(const struct motion_sensor_t *sensor)
 {
@@ -111,7 +118,8 @@ motion_sensor_time_to_read(const timestamp_t *ts,
 			  sensor->next_collection - motion_min_interval);
 }
 
-static enum sensor_config motion_sense_get_ec_config(void)
+STATIC_IF_NOT(CONFIG_ZTEST)
+enum sensor_config motion_sense_get_ec_config(void)
 {
 	switch (sensor_active) {
 	case SENSOR_ACTIVE_S0:
@@ -123,7 +131,7 @@ static enum sensor_config motion_sense_get_ec_config(void)
 	default:
 		CPRINTS("get_ec_config: Invalid active state: %x",
 			sensor_active);
-		return SENSOR_CONFIG_MAX;
+		return SENSOR_CONFIG_EC_S5;
 	}
 }
 /* motion_sense_set_data_rate
@@ -321,7 +329,12 @@ static void motion_sense_switch_sensor_rate(void)
 			body_detect_set_enable(false);
 			break;
 		case SENSOR_ACTIVE_S0:
-			body_detect_set_enable(was_enabled);
+			/* force to enable the body detection in S0 */
+			if (IS_ENABLED(
+				    CONFIG_BODY_DETECTION_ALWAYS_ENABLE_IN_S0))
+				body_detect_set_enable(true);
+			else
+				body_detect_set_enable(was_enabled);
 			break;
 		default:
 			break;
@@ -1422,7 +1435,8 @@ static enum ec_status host_cmd_motion_sense(struct host_cmd_handler_args *args)
 	default:
 		/* Call other users of the motion task */
 		if (IS_ENABLED(CONFIG_LID_ANGLE) &&
-		    (ret == EC_RES_INVALID_PARAM))
+		    (ret == EC_RES_INVALID_PARAM) &&
+		    sensor_board_is_lid_angle_available())
 			ret = host_cmd_motion_lid(args);
 		return ret;
 	}
diff --git a/common/motion_sense_fifo.c b/common/motion_sense_fifo.c
index d3887fa08f..fd1bfda88c 100644
--- a/common/motion_sense_fifo.c
+++ b/common/motion_sense_fifo.c
@@ -6,14 +6,14 @@
 #include "accelgyro.h"
 #include "console.h"
 #include "hwtimer.h"
+#include "math_util.h"
 #include "mkbp_event.h"
 #include "motion_sense_fifo.h"
+#include "online_calibration.h"
+#include "stdbool.h"
 #include "tablet_mode.h"
 #include "task.h"
 #include "util.h"
-#include "math_util.h"
-#include "online_calibration.h"
-#include "stdbool.h"
 
 #define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ##args)
 
diff --git a/common/newton_fit.c b/common/newton_fit.c
index 5d217bc63d..3ba01dbd90 100644
--- a/common/newton_fit.c
+++ b/common/newton_fit.c
@@ -5,9 +5,10 @@
 
 #include "common.h"
 #include "console.h"
-#include "newton_fit.h"
 #include "math.h"
 #include "math_util.h"
+#include "newton_fit.h"
+
 #include <string.h>
 
 #define CPRINTS(fmt, args...) cprints(CC_MOTION_SENSE, fmt, ##args)
diff --git a/common/ocpc.c b/common/ocpc.c
index c27cf4efe5..88308e5bc7 100644
--- a/common/ocpc.c
+++ b/common/ocpc.c
@@ -58,6 +58,7 @@ static int k_d = KD;
 static int k_p_div = KP_DIV;
 static int k_i_div = KI_DIV;
 static int k_d_div = KD_DIV;
+static int drive_limit = CONFIG_OCPC_DEF_DRIVELIMIT_MILLIVOLTS;
 static int debug_output;
 static int viz_output;
 
@@ -247,9 +248,10 @@ enum ec_error_list ocpc_calc_resistances(struct ocpc_data *ocpc,
 	return EC_SUCCESS;
 }
 
-int ocpc_config_secondary_charger(int *desired_input_current,
-				  struct ocpc_data *ocpc, int voltage_mv,
-				  int current_ma)
+int ocpc_config_secondary_charger(int *desired_charger_input_current,
+				  struct ocpc_data *ocpc,
+				  int desired_batt_voltage_mv,
+				  int desired_batt_current_ma)
 {
 	int rv = EC_SUCCESS;
 	struct batt_params batt;
@@ -285,8 +287,8 @@ int ocpc_config_secondary_charger(int *desired_input_current,
 
 	batt_info = battery_get_info();
 
-	if (current_ma == 0) {
-		vsys_target = voltage_mv;
+	if (desired_batt_current_ma == 0) {
+		vsys_target = desired_batt_voltage_mv;
 		goto set_vsys;
 	}
 
@@ -317,8 +319,8 @@ int ocpc_config_secondary_charger(int *desired_input_current,
 	if (!timestamp_expired(delay, NULL))
 		return EC_ERROR_BUSY;
 
-	result = charger_set_vsys_compensation(chgnum, ocpc, current_ma,
-					       voltage_mv);
+	result = charger_set_vsys_compensation(
+		chgnum, ocpc, desired_batt_current_ma, desired_batt_voltage_mv);
 	switch (result) {
 	case EC_SUCCESS:
 		/* No further action required, so we're done here. */
@@ -376,8 +378,9 @@ int ocpc_config_secondary_charger(int *desired_input_current,
 	if (batt.desired_voltage) {
 		if (((batt.voltage < batt_info->voltage_min) ||
 		     ((batt.voltage < batt_info->voltage_normal) &&
-		      (current_ma >= 0) &&
-		      (current_ma <= batt_info->precharge_current))) &&
+		      (desired_batt_current_ma >= 0) &&
+		      (desired_batt_current_ma <=
+		       batt_info->precharge_current))) &&
 		    (ph != PHASE_PRECHARGE)) {
 			/*
 			 * If the charger IC doesn't support the linear charge
@@ -389,11 +392,12 @@ int ocpc_config_secondary_charger(int *desired_input_current,
 			} else if (result == EC_SUCCESS) {
 				CPRINTS("OCPC: Enabling linear precharge");
 				ph = PHASE_PRECHARGE;
-				i_ma = current_ma;
+				i_ma = desired_batt_current_ma;
 			}
 		} else if (batt.voltage < batt.desired_voltage) {
 			if ((ph == PHASE_PRECHARGE) &&
-			    (current_ma > batt_info->precharge_current)) {
+			    (desired_batt_current_ma >
+			     batt_info->precharge_current)) {
 				/*
 				 * Precharge phase is complete.  Now set the
 				 * target VSYS to the battery voltage to prevent
@@ -422,7 +426,7 @@ int ocpc_config_secondary_charger(int *desired_input_current,
 
 			if ((ph != PHASE_PRECHARGE) && (ph < PHASE_CV_TRIP))
 				ph = PHASE_CC;
-			i_ma = current_ma;
+			i_ma = desired_batt_current_ma;
 		} else {
 			/*
 			 * Once the battery voltage reaches the desired voltage,
@@ -452,6 +456,16 @@ int ocpc_config_secondary_charger(int *desired_input_current,
 	 */
 	if (ocpc->last_vsys != OCPC_UNINIT) {
 		error = i_ma - batt.current;
+
+		/* Uses charger input error if controller is proportional only.
+		 */
+		if ((k_i == 0) && (k_d == 0)) {
+			int charger_input_error =
+				(*desired_charger_input_current -
+				 ocpc->secondary_ibus_ma);
+			error = MIN(error, charger_input_error);
+		}
+
 		/* Add some hysteresis. */
 		if (ABS(error) < (i_step / 2))
 			error = 0;
@@ -490,11 +504,15 @@ int ocpc_config_secondary_charger(int *desired_input_current,
 		 * VSYS rather quickly, but we'll be conservative on
 		 * increasing VSYS.
 		 */
-		if (drive > 10)
-			drive = 10;
+		if (drive > drive_limit)
+			drive = drive_limit;
 		CPRINTS_DBG("drive = %d", drive);
 	}
 
+	CPRINTS_DBG("##DATA = %d %d %d %d %d %d %d", batt.desired_current,
+		    batt.current, *desired_charger_input_current,
+		    ocpc->secondary_ibus_ma, error, ocpc->last_vsys, drive);
+
 	/*
 	 * For the pre-charge phase, simply keep the VSYS target at the desired
 	 * voltage.
@@ -537,7 +555,7 @@ int ocpc_config_secondary_charger(int *desired_input_current,
 
 	/* If we're input current limited, we cannot increase VSYS any more. */
 	CPRINTS_DBG("OCPC: Inst. Input Current: %dmA (Limit: %dmA)",
-		    ocpc->secondary_ibus_ma, *desired_input_current);
+		    ocpc->secondary_ibus_ma, *desired_charger_input_current);
 
 	if (charger_is_icl_reached(chgnum, &icl_reached) != EC_SUCCESS) {
 		/*
@@ -546,7 +564,7 @@ int ocpc_config_secondary_charger(int *desired_input_current,
 		 * 95% of the limit.
 		 */
 		if (ocpc->secondary_ibus_ma >=
-		    (*desired_input_current * 95 / 100))
+		    (*desired_charger_input_current * 95 / 100))
 			icl_reached = true;
 	}
 
@@ -765,3 +783,16 @@ static int command_ocpcpid(int argc, const char **argv)
 DECLARE_SAFE_CONSOLE_COMMAND(ocpcpid, command_ocpcpid,
 			     "[<k/p/d> <numerator> <denominator>]",
 			     "Show/Set PID constants for OCPC PID loop");
+
+static int command_ocpcdrvlmt(int argc, const char **argv)
+{
+	if (argc == 2) {
+		drive_limit = atoi(argv[1]);
+	}
+
+	/* Print the current constants */
+	ccprintf("Drive Limit = %d\n", drive_limit);
+	return EC_SUCCESS;
+}
+DECLARE_SAFE_CONSOLE_COMMAND(ocpcdrvlmt, command_ocpcdrvlmt, "[<drive_limit>]",
+			     "Show/Set drive limit for OCPC PID loop");
diff --git a/common/online_calibration.c b/common/online_calibration.c
index 2d79c6ae1d..a56aa0dce7 100644
--- a/common/online_calibration.c
+++ b/common/online_calibration.c
@@ -3,19 +3,19 @@
  * found in the LICENSE file.
  */
 
+#include "accel_cal.h"
 #include "accelgyro.h"
 #include "atomic.h"
-#include "hwtimer.h"
-#include "online_calibration.h"
 #include "common.h"
+#include "ec_commands.h"
+#include "gyro_cal.h"
+#include "hwtimer.h"
 #include "mag_cal.h"
+#include "mkbp_event.h"
+#include "online_calibration.h"
+#include "task.h"
 #include "util.h"
 #include "vec3.h"
-#include "task.h"
-#include "ec_commands.h"
-#include "accel_cal.h"
-#include "mkbp_event.h"
-#include "gyro_cal.h"
 
 #define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ##args)
 
diff --git a/common/panic_output.c b/common/panic_output.c
index 2f92e65514..55d551f548 100644
--- a/common/panic_output.c
+++ b/common/panic_output.c
@@ -175,8 +175,10 @@ uintptr_t get_panic_data_start(void)
 	if (IS_ENABLED(CONFIG_BOARD_NATIVE_POSIX))
 		return (uintptr_t)pdata_ptr;
 
+	/* LCOV_EXCL_START - Can't cover non posix lines (yet) */
 	return ((uintptr_t)CONFIG_PANIC_DATA_BASE + CONFIG_PANIC_DATA_SIZE -
 		pdata_ptr->struct_size);
+	/* LCOV_EXCL_STOP */
 }
 
 static uint32_t get_panic_data_size(void)
@@ -199,6 +201,7 @@ struct panic_data *get_panic_data_write(void)
 	return pdata_ptr;
 }
 #else
+/* LCOV_EXCL_START - Can't cover non posix lines (yet) */
 struct panic_data *get_panic_data_write(void)
 {
 	/*
@@ -208,7 +211,7 @@ struct panic_data *get_panic_data_write(void)
 	 * end of RAM.
 	 */
 	struct panic_data *const pdata_ptr = PANIC_DATA_PTR;
-	const struct jump_data *jdata_ptr;
+	struct jump_data *jdata_ptr;
 	uintptr_t data_begin;
 	size_t move_size;
 	int delta;
@@ -264,6 +267,16 @@ struct panic_data *get_panic_data_write(void)
 		move_size = 0;
 	}
 
+	/* Check if there's enough space for jump tags after move */
+	if (data_begin - move_size < JUMP_DATA_MIN_ADDRESS) {
+		/* Not enough room for jump tags, clear tags.
+		 * TODO(b/251190975): This failure should be reported
+		 * in the panic data structure for more visibility.
+		 */
+		move_size -= jdata_ptr->jump_tag_total;
+		jdata_ptr->jump_tag_total = 0;
+	}
+
 	data_begin -= move_size;
 
 	if (move_size != 0) {
@@ -282,6 +295,7 @@ struct panic_data *get_panic_data_write(void)
 
 	return pdata_ptr;
 }
+/* LCOV_EXCL_STOP */
 #endif /* CONFIG_BOARD_NATIVE_POSIX */
 
 static void panic_init(void)
diff --git a/common/power_button.c b/common/power_button.c
index 36d7e510d2..484d25154a 100644
--- a/common/power_button.c
+++ b/common/power_button.c
@@ -184,6 +184,15 @@ static void power_button_change_deferred(void)
 }
 DECLARE_DEFERRED(power_button_change_deferred);
 
+static void power_button_simulate_deferred(void)
+{
+	ccprintf("Simulating %s release.\n", power_button.name);
+	simulate_power_pressed = 0;
+	power_button_is_stable = 0;
+	power_button_change_deferred();
+}
+DECLARE_DEFERRED(power_button_simulate_deferred);
+
 void power_button_interrupt(enum gpio_signal signal)
 {
 	/*
@@ -205,15 +214,9 @@ void power_button_simulate_press(unsigned int duration)
 	ccprintf("Simulating %d ms %s press.\n", duration, power_button.name);
 	simulate_power_pressed = 1;
 	power_button_is_stable = 0;
-	hook_call_deferred(&power_button_change_deferred_data, 0);
-
-	if (duration > 0)
-		msleep(duration);
-
-	ccprintf("Simulating %s release.\n", power_button.name);
-	simulate_power_pressed = 0;
-	power_button_is_stable = 0;
-	hook_call_deferred(&power_button_change_deferred_data, 0);
+	power_button_change_deferred();
+	hook_call_deferred(&power_button_simulate_deferred_data,
+			   duration * MSEC);
 }
 
 /*****************************************************************************/
diff --git a/common/pwm_kblight.c b/common/pwm_kblight.c
index 5748bef700..bd7e07644d 100644
--- a/common/pwm_kblight.c
+++ b/common/pwm_kblight.c
@@ -19,11 +19,6 @@ static int kblight_pwm_set(int percent)
 	return EC_SUCCESS;
 }
 
-static int kblight_pwm_get(void)
-{
-	return pwm_get_duty(kblight_pwm_ch);
-}
-
 static int kblight_pwm_init(void)
 {
 	/* dnojiri: Why do we need save/restore setting over sysjump? */
@@ -46,7 +41,6 @@ static int kblight_pwm_get_enabled(void)
 const struct kblight_drv kblight_pwm = {
 	.init = kblight_pwm_init,
 	.set = kblight_pwm_set,
-	.get = kblight_pwm_get,
 	.enable = kblight_pwm_enable,
 	.get_enabled = kblight_pwm_get_enabled,
 };
diff --git a/common/rgb_keyboard.c b/common/rgb_keyboard.c
index 5f130ee776..0036cdcd16 100644
--- a/common/rgb_keyboard.c
+++ b/common/rgb_keyboard.c
@@ -2,8 +2,6 @@
  * Use of this source code is governed by a BSD-style license that can be
  * found in the LICENSE file.
  */
-#include <stdbool.h>
-
 #include "atomic.h"
 #include "common.h"
 #include "console.h"
@@ -18,6 +16,8 @@
 #include "timer.h"
 #include "util.h"
 
+#include <stdbool.h>
+
 /* Console output macros */
 #define CPUTS(outstr) cputs(CC_RGBKBD, outstr)
 #define CPRINTF(fmt, args...) cprintf(CC_RGBKBD, "RGBKBD: " fmt, ##args)
@@ -476,7 +476,6 @@ static void rgbkbd_reset(void)
 const struct kblight_drv kblight_rgbkbd = {
 	.init = rgbkbd_init,
 	.set = rgbkbd_kblight_set,
-	.get = NULL,
 	.enable = rgbkbd_enable,
 	.get_enabled = rgbkbd_get_enabled,
 };
diff --git a/common/shmalloc.c b/common/shmalloc.c
index 6cbf213227..37b7b2c736 100644
--- a/common/shmalloc.c
+++ b/common/shmalloc.c
@@ -5,8 +5,6 @@
  */
 
 /* Malloc/free memory module for Chrome EC */
-#include <stdint.h>
-
 #include "common.h"
 #include "hooks.h"
 #include "link_defs.h"
@@ -15,6 +13,8 @@
 #include "task.h"
 #include "util.h"
 
+#include <stdint.h>
+
 static struct mutex shmem_lock;
 
 #ifndef TEST_SHMALLOC
diff --git a/common/spi/flash_reg/CMakeLists.txt b/common/spi/flash_reg/CMakeLists.txt
index fda467d2e4..55708ac57e 100644
--- a/common/spi/flash_reg/CMakeLists.txt
+++ b/common/spi/flash_reg/CMakeLists.txt
@@ -6,6 +6,7 @@ cmake_minimum_required(VERSION 3.20.0)
 
 # Create the library
 if(BOARD STREQUAL unit_testing)
+  find_package(Zephyr COMPONENTS unittest REQUIRED HINTS $ENV{ZEPHYR_BASE})
   add_library(spi_flash_reg)
 else()
   zephyr_library_named(spi_flash_reg)
@@ -45,11 +46,8 @@ target_include_directories(spi_flash_reg PUBLIC public)
 if(BOARD STREQUAL unit_testing)
   project(flash_reg)
 
-  # Set the sources for the test (must be done before find_package)
-  set(SOURCES src/spi_flash_reg_test.c)
-
-  # Create the unittest libraries and binary
-  find_package(Zephyr COMPONENTS unittest REQUIRED HINTS $ENV{ZEPHYR_BASE})
+  # Set the sources for the test
+  target_sources(testbinary PRIVATE src/spi_flash_reg_test.c)
 
   # Link the library being tested
   target_link_libraries(testbinary PRIVATE spi_flash_reg)
diff --git a/common/spi/flash_reg/src/spi_flash_reg_test.c b/common/spi/flash_reg/src/spi_flash_reg_test.c
index 4a6a7a5edf..b9a45476b1 100644
--- a/common/spi/flash_reg/src/spi_flash_reg_test.c
+++ b/common/spi/flash_reg/src/spi_flash_reg_test.c
@@ -3,11 +3,11 @@
  * found in the LICENSE file.
  */
 
-#include <zephyr/ztest.h>
-
 #include "common.h"
 #include "spi_flash_reg.h"
 
+#include <zephyr/ztest.h>
+
 ZTEST_SUITE(flash_reg_to_protect, NULL, NULL, NULL, NULL, NULL);
 
 ZTEST(flash_reg_to_protect, test_invalid_args)
diff --git a/common/spi_flash.c b/common/spi_flash.c
index 8eaf3e8406..1540c7bb33 100644
--- a/common/spi_flash.c
+++ b/common/spi_flash.c
@@ -9,6 +9,8 @@
 #include "builtin/assert.h"
 #include "common.h"
 #include "console.h"
+#include "ec_commands.h"
+#include "flash.h"
 #include "host_command.h"
 #include "shared_mem.h"
 #include "spi.h"
@@ -17,8 +19,6 @@
 #include "timer.h"
 #include "util.h"
 #include "watchdog.h"
-#include "ec_commands.h"
-#include "flash.h"
 
 /*
  * Time to sleep when chip is busy
diff --git a/common/spi_nor.c b/common/spi_nor.c
index 015cb7077c..1eca5d806f 100644
--- a/common/spi_nor.c
+++ b/common/spi_nor.c
@@ -7,13 +7,13 @@
 
 #include "common.h"
 #include "console.h"
-#include "spi_nor.h"
+#include "sfdp.h"
 #include "shared_mem.h"
-#include "util.h"
-#include "task.h"
 #include "spi.h"
-#include "sfdp.h"
+#include "spi_nor.h"
+#include "task.h"
 #include "timer.h"
+#include "util.h"
 #include "watchdog.h"
 
 #ifdef CONFIG_SPI_NOR_DEBUG
diff --git a/common/stillness_detector.c b/common/stillness_detector.c
index c43e19873e..f81c51ef3a 100644
--- a/common/stillness_detector.c
+++ b/common/stillness_detector.c
@@ -6,6 +6,7 @@
 #include "common.h"
 #include "stillness_detector.h"
 #include "timer.h"
+
 #include <string.h>
 
 static void still_det_reset(struct still_det *still_det)
diff --git a/common/system.c b/common/system.c
index 375cca2882..e10712259c 100644
--- a/common/system.c
+++ b/common/system.c
@@ -27,6 +27,7 @@
 #ifdef CONFIG_MPU
 #include "mpu.h"
 #endif
+#include "cros_version.h"
 #include "panic.h"
 #include "sysjump.h"
 #include "system.h"
@@ -36,7 +37,6 @@
 #include "usb_pd.h"
 #include "usb_pd_tcpm.h"
 #include "util.h"
-#include "cros_version.h"
 #include "watchdog.h"
 
 /* Console output macros */
@@ -67,6 +67,11 @@ static enum ec_reboot_cmd reboot_at_shutdown;
 
 static enum sysinfo_flags system_info_flags;
 
+/* Ensure enough space for panic_data, jump_data and at least one jump tag */
+BUILD_ASSERT((sizeof(struct panic_data) + sizeof(struct jump_data) +
+	      JUMP_TAG_MAX_SIZE) <= CONFIG_PRESERVED_END_OF_RAM_SIZE,
+	     "End of ram data size is too small for panic and jump data");
+
 STATIC_IF(CONFIG_HIBERNATE) uint32_t hibernate_seconds;
 STATIC_IF(CONFIG_HIBERNATE) uint32_t hibernate_microseconds;
 
@@ -349,15 +354,24 @@ test_mockable int system_jumped_late(void)
 int system_add_jump_tag(uint16_t tag, int version, int size, const void *data)
 {
 	struct jump_tag *t;
+	size_t new_entry_size;
 
 	/* Only allowed during a sysjump */
 	if (!jdata || jdata->magic != JUMP_DATA_MAGIC)
 		return EC_ERROR_UNKNOWN;
 
 	/* Make room for the new tag */
-	if (size > 255)
+	if (size > JUMP_TAG_MAX_SIZE)
+		return EC_ERROR_INVAL;
+
+	new_entry_size = ROUNDUP4(size) + sizeof(struct jump_tag);
+
+	if (system_usable_ram_end() - new_entry_size < JUMP_DATA_MIN_ADDRESS) {
+		ccprintf("ERROR: out of space for jump tags\n");
 		return EC_ERROR_INVAL;
-	jdata->jump_tag_total += ROUNDUP4(size) + sizeof(struct jump_tag);
+	}
+
+	jdata->jump_tag_total += new_entry_size;
 
 	t = (struct jump_tag *)system_usable_ram_end();
 	t->tag = tag;
@@ -378,6 +392,10 @@ test_mockable const uint8_t *system_get_jump_tag(uint16_t tag, int *version,
 	if (!jdata)
 		return NULL;
 
+	/* Ensure system_usable_ram_end() is within bounds */
+	if (system_usable_ram_end() < JUMP_DATA_MIN_ADDRESS)
+		return NULL;
+
 	/* Search through tag data for a match */
 	while (used < jdata->jump_tag_total) {
 		/* Check the next tag */
@@ -918,6 +936,18 @@ void system_common_pre_init(void)
 		else
 			delta = sizeof(struct jump_data) - jdata->struct_size;
 
+		/*
+		 * Check if enough space for jump data.
+		 * Clear jump data and return if not.
+		 */
+		if (system_usable_ram_end() < JUMP_DATA_MIN_ADDRESS) {
+			/* TODO(b/251190975): This failure should be reported
+			 * in the panic data structure for more visibility.
+			 */
+			memset(jdata, 0, sizeof(struct jump_data));
+			return;
+		}
+
 		if (delta && jdata->jump_tag_total) {
 			uint8_t *d = (uint8_t *)system_usable_ram_end();
 			memmove(d, d + delta, jdata->jump_tag_total);
diff --git a/common/tablet_mode.c b/common/tablet_mode.c
index 576e80c0ef..18e97e91db 100644
--- a/common/tablet_mode.c
+++ b/common/tablet_mode.c
@@ -28,6 +28,12 @@ static uint32_t tablet_mode;
  */
 static bool tablet_mode_forced;
 
+/*
+ * Console command can force the value of tablet_mode. If tablet_mode_force is
+ * false, use stored tablet mode value before it was (possibly) overridden.
+ */
+static uint32_t tablet_mode_store;
+
 /* True if GMR sensor is reporting 360 degrees. */
 static bool gmr_sensor_at_360;
 
@@ -62,27 +68,51 @@ static void notify_tablet_mode_change(void)
 
 void tablet_set_mode(int mode, uint32_t trigger)
 {
-	uint32_t old_mode = tablet_mode;
-
-	/* If tablet_mode is forced via a console command, ignore set. */
-	if (tablet_mode_forced)
-		return;
+	uint32_t new_mode = tablet_mode_forced ? tablet_mode_store :
+						 tablet_mode;
+	uint32_t old_mode = 0;
 
 	if (disabled) {
-		CPRINTS("Tablet mode set while disabled (ignoring)!");
+		/*
+		 * If tablet mode is being forced by the user, then this logging
+		 * would be misleading since the mode wouldn't change anyway, so
+		 * skip it.
+		 */
+		if (!tablet_mode_forced)
+			CPRINTS("Tablet mode set while disabled (ignoring)!");
 		return;
 	}
 
 	if (gmr_sensor_at_360 && !mode) {
-		CPRINTS("Ignoring tablet mode exit while gmr sensor "
-			"reports 360-degree tablet mode.");
+		/*
+		 * If tablet mode is being forced by the user, then this logging
+		 * would be misleading since the mode wouldn't change anyway, so
+		 * skip it.
+		 */
+		if (!tablet_mode_forced)
+			CPRINTS("Ignoring tablet mode exit while gmr sensor "
+				"reports 360-degree tablet mode.");
 		return;
 	}
 
 	if (mode)
-		tablet_mode |= trigger;
+		new_mode |= trigger;
 	else
-		tablet_mode &= ~trigger;
+		new_mode &= ~trigger;
+
+	if (tablet_mode_forced) {
+		/*
+		 * Save the current mode based on the HW orientation, so we
+		 * apply the correct mode if tablet mode no longer forced in the
+		 * future. Don't notify of the tablet mode change yet, since
+		 * that will be done as part of handling 'tabletmode reset'.
+		 */
+		tablet_mode_store = new_mode;
+		return;
+	}
+
+	old_mode = tablet_mode;
+	tablet_mode = new_mode;
 
 	/* Boolean comparison */
 	if (!tablet_mode == !old_mode)
@@ -173,11 +203,42 @@ void gmr_tablet_switch_disable(void)
 }
 #endif
 
+static enum ec_status tablet_mode_command(struct host_cmd_handler_args *args)
+{
+	const struct ec_params_set_tablet_mode *p = args->params;
+
+	if (tablet_mode_forced == false)
+		tablet_mode_store = tablet_mode;
+
+	switch (p->tablet_mode) {
+	case TABLET_MODE_DEFAULT:
+		tablet_mode = tablet_mode_store;
+		tablet_mode_forced = false;
+		break;
+	case TABLET_MODE_FORCE_TABLET:
+		tablet_mode = TABLET_TRIGGER_LID;
+		tablet_mode_forced = true;
+		break;
+	case TABLET_MODE_FORCE_CLAMSHELL:
+		tablet_mode = 0;
+		tablet_mode_forced = true;
+		break;
+	default:
+		CPRINTS("Invalid EC_CMD_SET_TABLET_MODE parameter: %d",
+			p->tablet_mode);
+		return EC_RES_INVALID_PARAM;
+	}
+
+	notify_tablet_mode_change();
+
+	return EC_RES_SUCCESS;
+}
+DECLARE_HOST_COMMAND(EC_CMD_SET_TABLET_MODE, tablet_mode_command,
+		     EC_VER_MASK(0) | EC_VER_MASK(1));
+
 #ifdef CONFIG_TABLET_MODE
 static int command_settabletmode(int argc, const char **argv)
 {
-	static uint32_t tablet_mode_store;
-
 	if (argc == 1) {
 		print_tablet_mode();
 		return EC_SUCCESS;
diff --git a/common/test_util.c b/common/test_util.c
index 659476fc17..fd8caba10a 100644
--- a/common/test_util.c
+++ b/common/test_util.c
@@ -11,7 +11,6 @@
 #include <stdlib.h>
 #endif
 
-#include <assert.h>
 #include "console.h"
 #include "hooks.h"
 #include "host_command.h"
@@ -20,6 +19,8 @@
 #include "test_util.h"
 #include "util.h"
 
+#include <assert.h>
+
 struct test_util_tag {
 	uint8_t error_count;
 };
diff --git a/common/timer.c b/common/timer.c
index d0fd1eca6e..bf2a731a1d 100644
--- a/common/timer.c
+++ b/common/timer.c
@@ -12,9 +12,9 @@
 #include "hooks.h"
 #include "hwtimer.h"
 #include "system.h"
-#include "util.h"
 #include "task.h"
 #include "timer.h"
+#include "util.h"
 #include "watchdog.h"
 
 #ifdef CONFIG_ZEPHYR
diff --git a/common/uart_buffering.c b/common/uart_buffering.c
index 1aa6e9ef35..f3d150dfda 100644
--- a/common/uart_buffering.c
+++ b/common/uart_buffering.c
@@ -5,8 +5,6 @@
 
 /* Common code to do UART buffering and printing */
 
-#include <stdarg.h>
-
 #include "common.h"
 #include "console.h"
 #include "hooks.h"
@@ -19,6 +17,8 @@
 #include "uart.h"
 #include "util.h"
 
+#include <stdarg.h>
+
 /* Macros to advance in the circular buffers */
 #define TX_BUF_NEXT(i) (((i) + 1) & (CONFIG_UART_TX_BUF_SIZE - 1))
 #define RX_BUF_NEXT(i) (((i) + 1) & (CONFIG_UART_RX_BUF_SIZE - 1))
diff --git a/common/uart_hostcmd.c b/common/uart_hostcmd.c
index 7b4ff5b461..506a49221d 100644
--- a/common/uart_hostcmd.c
+++ b/common/uart_hostcmd.c
@@ -3,8 +3,8 @@
  * found in the LICENSE file.
  */
 
-#include "config.h"
 #include "common.h"
+#include "config.h"
 #include "ec_commands.h"
 #include "host_command.h"
 #include "uart.h"
diff --git a/common/uart_printf.c b/common/uart_printf.c
index 6f8ebb5cbc..55b8a40cde 100644
--- a/common/uart_printf.c
+++ b/common/uart_printf.c
@@ -3,12 +3,12 @@
  * found in the LICENSE file.
  */
 
-#include <stddef.h>
-
 #include "common.h"
 #include "printf.h"
 #include "uart.h"
 
+#include <stddef.h>
+
 static int __tx_char(void *context, int c)
 {
 	/*
@@ -31,9 +31,10 @@ int uart_putc(int c)
 int uart_puts(const char *outstr)
 {
 	/* Put all characters in the output buffer */
-	while (*outstr) {
-		if (__tx_char(NULL, *outstr++) != 0)
+	for (; *outstr != '\0'; ++outstr) {
+		if (__tx_char(NULL, *outstr) != 0) {
 			break;
+		}
 	}
 
 	uart_tx_start();
@@ -44,30 +45,34 @@ int uart_puts(const char *outstr)
 
 int uart_put(const char *out, int len)
 {
+	int written;
+
 	/* Put all characters in the output buffer */
-	while (len--) {
-		if (__tx_char(NULL, *out++) != 0)
+	for (written = 0; written < len; written++) {
+		if (__tx_char(NULL, *out++) != 0) {
 			break;
+		}
 	}
 
 	uart_tx_start();
 
-	/* Successful if we consumed all output */
-	return len ? EC_ERROR_OVERFLOW : EC_SUCCESS;
+	return written;
 }
 
 int uart_put_raw(const char *out, int len)
 {
+	int written;
+
 	/* Put all characters in the output buffer */
-	while (len--) {
-		if (uart_tx_char_raw(NULL, *out++) != 0)
+	for (written = 0; written < len; written++) {
+		if (uart_tx_char_raw(NULL, *out++) != 0) {
 			break;
+		}
 	}
 
 	uart_tx_start();
 
-	/* Successful if we consumed all output */
-	return len ? EC_ERROR_OVERFLOW : EC_SUCCESS;
+	return written;
 }
 
 int uart_vprintf(const char *format, va_list args)
diff --git a/common/uptime.c b/common/uptime.c
index 3b2c68fd34..1ce7e2a329 100644
--- a/common/uptime.c
+++ b/common/uptime.c
@@ -2,13 +2,13 @@
  * Use of this source code is governed by a BSD-style license that can be
  * found in the LICENSE file.
  */
-#include <stddef.h>
-
 #include "chipset.h"
-#include "system.h"
 #include "host_command.h"
+#include "system.h"
 #include "util.h"
 
+#include <stddef.h>
+
 static enum ec_status
 host_command_get_uptime_info(struct host_cmd_handler_args *args)
 {
diff --git a/common/usb_common.c b/common/usb_common.c
index b4fefb6cf7..a92b1e380f 100644
--- a/common/usb_common.c
+++ b/common/usb_common.c
@@ -17,9 +17,9 @@
 #include "ec_commands.h"
 #include "gpio.h"
 #include "hooks.h"
+#include "host_command.h"
 #include "mkbp_event.h"
 #include "stdbool.h"
-#include "host_command.h"
 #include "system.h"
 #include "task.h"
 #include "typec_control.h"
@@ -27,7 +27,7 @@
 #include "usb_common.h"
 #include "usb_mux.h"
 #include "usb_pd.h"
-#include "usb_pd_dpm.h"
+#include "usb_pd_dpm_sm.h"
 #include "usb_pd_flags.h"
 #include "usb_pd_tcpm.h"
 #include "usb_pe_sm.h"
@@ -198,18 +198,6 @@ enum pd_cc_states pd_get_cc_state(enum tcpc_cc_voltage_status cc1,
 	return PD_CC_NONE;
 }
 
-/**
- * This function checks the current CC status of the port partner
- * and returns true if the attached partner is debug accessory.
- */
-bool pd_is_debug_acc(int port)
-{
-	enum pd_cc_states cc_state = pd_get_task_cc_state(port);
-
-	return cc_state == PD_CC_UFP_DEBUG_ACC ||
-	       cc_state == PD_CC_DFP_DEBUG_ACC;
-}
-
 __overridable int pd_board_check_request(uint32_t rdo, int pdo_cnt)
 {
 	return EC_SUCCESS;
@@ -634,15 +622,17 @@ void pd_set_vbus_discharge(int port, int enable)
 	static mutex_t discharge_lock[CONFIG_USB_PD_PORT_MAX_COUNT];
 #ifdef CONFIG_ZEPHYR
 	static bool inited[CONFIG_USB_PD_PORT_MAX_COUNT];
+#endif
 
+	if (port >= board_get_usb_pd_port_count())
+		return;
+
+#ifdef CONFIG_ZEPHYR
 	if (!inited[port]) {
 		(void)k_mutex_init(&discharge_lock[port]);
 		inited[port] = true;
 	}
 #endif
-	if (port >= board_get_usb_pd_port_count())
-		return;
-
 	mutex_lock(&discharge_lock[port]);
 	enable &= !board_vbus_source_enabled(port);
 
diff --git a/common/usb_host_command.c b/common/usb_host_command.c
index 83a6e24a7d..0aeff40d39 100644
--- a/common/usb_host_command.c
+++ b/common/usb_host_command.c
@@ -7,13 +7,13 @@
 #include "console.h"
 #include "consumer.h"
 #include "ec_commands.h"
-#include "queue_policies.h"
 #include "host_command.h"
 #include "printf.h"
+#include "queue_policies.h"
 #include "system.h"
+#include "usb-stream.h"
 #include "usb_api.h"
 #include "usb_hw.h"
-#include "usb-stream.h"
 #include "util.h"
 
 #define CPUTS(outstr) cputs(CC_USB, outstr)
diff --git a/common/usb_i2c.c b/common/usb_i2c.c
index 8482ec0967..b1cd45c76a 100644
--- a/common/usb_i2c.c
+++ b/common/usb_i2c.c
@@ -4,21 +4,19 @@
  */
 
 #include "common.h"
-#include "link_defs.h"
-#include "registers.h"
-#include "i2c.h"
-#include "usb_descriptor.h"
-#include "util.h"
-
-#include "common.h"
 #include "console.h"
 #include "consumer.h"
+#include "i2c.h"
+#include "link_defs.h"
 #include "producer.h"
 #include "queue.h"
 #include "queue_policies.h"
+#include "registers.h"
 #include "task.h"
 #include "usb-stream.h"
+#include "usb_descriptor.h"
 #include "usb_i2c.h"
+#include "util.h"
 
 #define CPRINTS(format, args...) cprints(CC_I2C, format, ##args)
 
diff --git a/common/usb_pd_alt_mode_dfp.c b/common/usb_pd_alt_mode_dfp.c
index dc8543c920..6df0215274 100644
--- a/common/usb_pd_alt_mode_dfp.c
+++ b/common/usb_pd_alt_mode_dfp.c
@@ -13,8 +13,8 @@
 #include "task_id.h"
 #include "timer.h"
 #include "typec_control.h"
-#include "usb_common.h"
 #include "usb_charge.h"
+#include "usb_common.h"
 #include "usb_dp_alt_mode.h"
 #include "usb_mux.h"
 #include "usb_pd.h"
diff --git a/common/usb_pd_console_cmd.c b/common/usb_pd_console_cmd.c
index 23f2a27b1e..5a1afe88a5 100644
--- a/common/usb_pd_console_cmd.c
+++ b/common/usb_pd_console_cmd.c
@@ -7,8 +7,8 @@
 
 #include "console.h"
 #include "usb_pd.h"
-#include "util.h"
 #include "usb_pd_tcpm.h"
+#include "util.h"
 
 #ifdef CONFIG_USB_PD_ALT_MODE_DFP
 #ifdef CONFIG_CMD_USB_PD_PE
diff --git a/common/usb_pd_host_cmd.c b/common/usb_pd_host_cmd.c
index e3008d26fb..ea05de3457 100644
--- a/common/usb_pd_host_cmd.c
+++ b/common/usb_pd_host_cmd.c
@@ -5,8 +5,6 @@
  * Host commands for USB-PD module.
  */
 
-#include <string.h>
-
 #include "atomic.h"
 #include "battery.h"
 #include "charge_manager.h"
@@ -17,8 +15,10 @@
 #include "tcpm/tcpm.h"
 #include "usb_common.h"
 #include "usb_mux.h"
-#include "usb_pd_tcpm.h"
 #include "usb_pd.h"
+#include "usb_pd_tcpm.h"
+
+#include <string.h>
 #ifdef CONFIG_COMMON_RUNTIME
 #define CPRINTF(format, args...) cprintf(CC_USBPD, format, ##args)
 #define CPRINTS(format, args...) cprints(CC_USBPD, format, ##args)
diff --git a/common/usb_pd_pdo.c b/common/usb_pd_pdo.c
index 3141af34a9..66f5f3feac 100644
--- a/common/usb_pd_pdo.c
+++ b/common/usb_pd_pdo.c
@@ -4,8 +4,8 @@
  */
 
 #include "usb_pd.h"
-#include "util.h"
 #include "usb_pd_pdo.h"
+#include "util.h"
 
 #ifndef CONFIG_USB_PD_CUSTOM_PDO
 
diff --git a/common/usb_pd_policy.c b/common/usb_pd_policy.c
index c63cde6301..036e253766 100644
--- a/common/usb_pd_policy.c
+++ b/common/usb_pd_policy.c
@@ -22,13 +22,13 @@
 #include "task.h"
 #include "tcpm/tcpm.h"
 #include "timer.h"
-#include "util.h"
 #include "usb_api.h"
 #include "usb_common.h"
 #include "usb_mux.h"
 #include "usb_pd.h"
 #include "usb_pd_tcpm.h"
 #include "usbc_ppc.h"
+#include "util.h"
 
 #ifdef CONFIG_COMMON_RUNTIME
 #define CPRINTS(format, args...) cprints(CC_USBPD, format, ##args)
diff --git a/common/usb_pd_protocol.c b/common/usb_pd_protocol.c
index 6ad8f7a9f8..c946f417f8 100644
--- a/common/usb_pd_protocol.c
+++ b/common/usb_pd_protocol.c
@@ -26,16 +26,16 @@
 #include "tcpm/tcpm.h"
 #include "timer.h"
 #include "typec_control.h"
-#include "util.h"
 #include "usb_charge.h"
 #include "usb_common.h"
 #include "usb_mux.h"
 #include "usb_pd.h"
 #include "usb_pd_flags.h"
-#include "usb_pd_tcpm.h"
 #include "usb_pd_tcpc.h"
+#include "usb_pd_tcpm.h"
 #include "usbc_ocp.h"
 #include "usbc_ppc.h"
+#include "util.h"
 #include "vboot.h"
 
 /* Flags to clear on a disconnect */
diff --git a/common/usb_pd_tcpc.c b/common/usb_pd_tcpc.c
index c8010a5005..a9a4d1d5a5 100644
--- a/common/usb_pd_tcpc.c
+++ b/common/usb_pd_tcpc.c
@@ -18,10 +18,10 @@
 #include "tcpm/tcpci.h"
 #include "tcpm/tcpm.h"
 #include "timer.h"
-#include "util.h"
 #include "usb_pd.h"
 #include "usb_pd_config.h"
 #include "usb_pd_tcpm.h"
+#include "util.h"
 
 #ifdef CONFIG_COMMON_RUNTIME
 #define CPRINTF(format, args...) cprintf(CC_USBPD, format, ##args)
diff --git a/common/usb_update.c b/common/usb_update.c
index 444a4d6949..2e9a681fed 100644
--- a/common/usb_update.c
+++ b/common/usb_update.c
@@ -9,8 +9,8 @@
 #include "consumer.h"
 #include "curve25519.h"
 #include "flash.h"
-#include "queue_policies.h"
 #include "host_command.h"
+#include "queue_policies.h"
 #include "rollback.h"
 #include "rwsig.h"
 #include "sha256.h"
diff --git a/common/usbc/dp_alt_mode.c b/common/usbc/dp_alt_mode.c
index f53d212b91..22975aff5b 100644
--- a/common/usbc/dp_alt_mode.c
+++ b/common/usbc/dp_alt_mode.c
@@ -9,8 +9,6 @@
  * section 5.2
  */
 
-#include <stdbool.h>
-#include <stdint.h>
 #include "atomic.h"
 #include "builtin/assert.h"
 #include "console.h"
@@ -19,6 +17,9 @@
 #include "usb_pd.h"
 #include "usb_pd_tcpm.h"
 
+#include <stdbool.h>
+#include <stdint.h>
+
 #ifdef CONFIG_COMMON_RUNTIME
 #define CPRINTF(format, args...) cprintf(CC_USBPD, format, ##args)
 #define CPRINTS(format, args...) cprints(CC_USBPD, format, ##args)
@@ -306,13 +307,13 @@ enum dpm_msg_setup_status dp_setup_next_vdm(int port, int *vdo_count,
 		 * this doesn't set up the VDM, it clears state.
 		 * TODO(b/159856063): Clean up the API to the fx functions.
 		 */
-		if (!(modep && modep->opos))
-			return MSG_SETUP_ERROR;
-
 		usb_mux_set_safe_mode_exit(port);
 		dp_state[port] = DP_PREPARE_EXIT;
 		return MSG_SETUP_MUX_WAIT;
 	case DP_PREPARE_EXIT:
+		if (!(modep && modep->opos))
+			return MSG_SETUP_ERROR;
+
 		/* DPM should call setup only after safe state is set */
 		vdm[0] = VDO(USB_SID_DISPLAYPORT, 1, /* structured */
 			     CMD_EXIT_MODE);
diff --git a/common/usbc/tbt_alt_mode.c b/common/usbc/tbt_alt_mode.c
index c0e4e172cc..fec1ee9de3 100644
--- a/common/usbc/tbt_alt_mode.c
+++ b/common/usbc/tbt_alt_mode.c
@@ -9,8 +9,6 @@
  */
 
 #include "atomic.h"
-#include <stdbool.h>
-#include <stdint.h>
 #include "compile_time_macros.h"
 #include "console.h"
 #include "tcpm/tcpm.h"
@@ -23,6 +21,9 @@
 #include "usb_pe_sm.h"
 #include "usb_tbt_alt_mode.h"
 
+#include <stdbool.h>
+#include <stdint.h>
+
 /*
  * Enter/Exit TBT mode with active cable
  *
diff --git a/common/usbc/usb_mode.c b/common/usbc/usb_mode.c
index 8f4824e4e9..017f89fc74 100644
--- a/common/usbc/usb_mode.c
+++ b/common/usbc/usb_mode.c
@@ -9,8 +9,6 @@
  * USB Power Delivery Specification Revision 3.0, Version 2.0 Section 6.4.8
  */
 
-#include <stdbool.h>
-#include <stdint.h>
 #include "compile_time_macros.h"
 #include "console.h"
 #include "tcpm/tcpm.h"
@@ -19,12 +17,15 @@
 #include "usb_mode.h"
 #include "usb_mux.h"
 #include "usb_pd.h"
-#include "usb_pd_dpm.h"
+#include "usb_pd_dpm_sm.h"
 #include "usb_pd_tcpm.h"
 #include "usb_pe_sm.h"
 #include "usb_tbt_alt_mode.h"
 #include "usbc_ppc.h"
 
+#include <stdbool.h>
+#include <stdint.h>
+
 #ifdef CONFIG_COMMON_RUNTIME
 #define CPRINTF(format, args...) cprintf(CC_USBPD, format, ##args)
 #define CPRINTS(format, args...) cprints(CC_USBPD, format, ##args)
diff --git a/common/usbc/usb_pd_console.c b/common/usbc/usb_pd_console.c
index 89a5492ad5..ed3d687ef4 100644
--- a/common/usbc/usb_pd_console.c
+++ b/common/usbc/usb_pd_console.c
@@ -6,12 +6,12 @@
 #include "common.h"
 #include "console.h"
 #include "usb_common.h"
-#include "usb_pd_dpm.h"
+#include "usb_pd.h"
+#include "usb_pd_dpm_sm.h"
 #include "usb_pd_timer.h"
 #include "usb_pe_sm.h"
 #include "usb_prl_sm.h"
 #include "usb_tc_sm.h"
-#include "usb_pd.h"
 #include "util.h"
 
 #ifndef TEST_USB_PD_CONSOLE
@@ -146,8 +146,8 @@ static
 				case PD_DRP_FORCE_SOURCE:
 					ccprintf("force source\n");
 					break;
-					cflush();
 				}
+				cflush();
 			} else {
 				if (!strcasecmp(argv[3], "on"))
 					pd_set_dual_role(port,
diff --git a/common/usbc/usb_pd_dpm.c b/common/usbc/usb_pd_dpm.c
index daccc2ba2a..775ea0da46 100644
--- a/common/usbc/usb_pd_dpm.c
+++ b/common/usbc/usb_pd_dpm.c
@@ -25,12 +25,13 @@
 #include "usb_mode.h"
 #include "usb_mux.h"
 #include "usb_pd.h"
-#include "usb_pd_dpm.h"
+#include "usb_pd_dpm_sm.h"
 #include "usb_pd_pdo.h"
 #include "usb_pd_tcpm.h"
 #include "usb_pd_timer.h"
 #include "usb_pe_sm.h"
 #include "usb_tbt_alt_mode.h"
+#include "usb_tc_sm.h"
 
 #ifdef CONFIG_ZEPHYR
 #include "temp_sensor/temp_sensor.h"
@@ -48,6 +49,8 @@
 #define DPM_ATTENION_MAX_VDO 2
 
 static struct {
+	/* state machine context */
+	struct sm_ctx ctx;
 	atomic_t flags;
 	uint32_t vdm_req[VDO_MAX_SIZE];
 	int vdm_req_cnt;
@@ -69,10 +72,51 @@ static struct {
 #define DPM_FLAG_ENTER_ANY \
 	(DPM_FLAG_ENTER_DP | DPM_FLAG_ENTER_TBT | DPM_FLAG_ENTER_USB4)
 #define DPM_FLAG_SEND_VDM_REQ BIT(5)
-#define DPM_FLAG_DATA_RESET_REQUESTED BIT(6)
-#define DPM_FLAG_DATA_RESET_DONE BIT(7)
-#define DPM_FLAG_PD_BUTTON_PRESSED BIT(8)
-#define DPM_FLAG_PD_BUTTON_RELEASED BIT(9)
+#define DPM_FLAG_DATA_RESET_DONE BIT(6)
+#define DPM_FLAG_PD_BUTTON_PRESSED BIT(7)
+#define DPM_FLAG_PD_BUTTON_RELEASED BIT(8)
+#define DPM_FLAG_PE_READY BIT(9)
+
+/* List of all Device Policy Manager level states */
+enum usb_dpm_state {
+	/* Normal States */
+	DPM_WAITING,
+	DPM_DFP_READY,
+	DPM_UFP_READY,
+	DPM_DATA_RESET,
+};
+
+/* Forward declare the full list of states. This is indexed by usb_pd_state */
+static const struct usb_state dpm_states[];
+
+/* List of human readable state names for console debugging */
+__maybe_unused static __const_data const char *const dpm_state_names[] = {
+	/* Normal States */
+	[DPM_WAITING] = "DPM Waiting",
+	[DPM_DFP_READY] = "DPM DFP Ready",
+	[DPM_UFP_READY] = "DPM UFP Ready",
+	[DPM_DATA_RESET] = "DPM Data Reset",
+};
+
+static enum sm_local_state local_state[CONFIG_USB_PD_PORT_MAX_COUNT];
+
+/* Set the DPM state machine to a new state. */
+static void set_state_dpm(const int port, const enum usb_dpm_state new_state)
+{
+	set_state(port, &dpm[port].ctx, &dpm_states[new_state]);
+}
+
+/* Get the current TypeC state. */
+__maybe_unused test_export_static enum usb_dpm_state
+get_state_dpm(const int port)
+{
+	return dpm[port].ctx.current - &dpm_states[0];
+}
+
+static void print_current_state(const int port)
+{
+	CPRINTS("C%d: %s", port, dpm_state_names[get_state_dpm(port)]);
+}
 
 #ifdef CONFIG_ZEPHYR
 static int init_vdm_req_mutex(const struct device *dev)
@@ -180,6 +224,9 @@ void dpm_init(int port)
 {
 	dpm[port].flags = 0;
 	dpm[port].pd_button_state = DPM_PD_BUTTON_IDLE;
+
+	/* Ensure that DPM state machine gets reset */
+	set_state_dpm(port, DPM_WAITING);
 }
 
 void dpm_mode_exit_complete(int port)
@@ -203,11 +250,22 @@ void dpm_set_mode_exit_request(int port)
 
 void dpm_data_reset_complete(int port)
 {
-	DPM_CLR_FLAG(port, DPM_FLAG_DATA_RESET_REQUESTED);
 	DPM_SET_FLAG(port, DPM_FLAG_DATA_RESET_DONE);
 	DPM_CLR_FLAG(port, DPM_FLAG_MODE_ENTRY_DONE);
 }
 
+void dpm_set_pe_ready(int port, bool enable)
+{
+	/*
+	 * DPM should remain DPM_WAITING state until the PE is in its ready
+	 * state and is able to accept requests from the DPM layer.
+	 */
+	if (enable)
+		DPM_SET_FLAG(port, DPM_FLAG_PE_READY);
+	else
+		DPM_CLR_FLAG(port, DPM_FLAG_PE_READY);
+}
+
 static void dpm_clear_mode_exit_request(int port)
 {
 	DPM_CLR_FLAG(port, DPM_FLAG_EXIT_REQUEST);
@@ -286,236 +344,6 @@ void dpm_vdm_naked(int port, enum tcpci_msg_type type, uint16_t svid,
 	}
 }
 
-/*
- * Requests that the PE send one VDM, whichever is next in the mode entry
- * sequence. This only happens if preconditions for mode entry are met. If
- * CONFIG_USB_PD_REQUIRE_AP_MODE_ENTRY is enabled, this function waits for the
- * AP to direct mode entry.
- */
-static void dpm_attempt_mode_entry(int port)
-{
-	int vdo_count = 0;
-	uint32_t vdm[VDO_MAX_SIZE];
-	enum tcpci_msg_type tx_type = TCPCI_MSG_SOP;
-	bool enter_mode_requested =
-		IS_ENABLED(CONFIG_USB_PD_REQUIRE_AP_MODE_ENTRY) ? false : true;
-	enum dpm_msg_setup_status status = MSG_SETUP_UNSUPPORTED;
-
-	if (pd_get_data_role(port) != PD_ROLE_DFP) {
-		if (DPM_CHK_FLAG(port, DPM_FLAG_ENTER_DP | DPM_FLAG_ENTER_TBT |
-					       DPM_FLAG_ENTER_USB4))
-			DPM_CLR_FLAG(port, DPM_FLAG_ENTER_DP |
-						   DPM_FLAG_ENTER_TBT |
-						   DPM_FLAG_ENTER_USB4);
-		/*
-		 * TODO(b/168030639): Notify the AP that the enter mode request
-		 * failed.
-		 */
-		return;
-	}
-
-#ifdef CONFIG_AP_POWER_CONTROL
-	/*
-	 * Do not try to enter mode while CPU is off.
-	 * CPU transitions (e.g b/158634281) can occur during the discovery
-	 * phase or during enter/exit negotiations, and the state
-	 * of the modes can get out of sync, causing the attempt to
-	 * enter the mode to fail prematurely.
-	 */
-	if (!chipset_in_state(CHIPSET_STATE_ANY_SUSPEND | CHIPSET_STATE_ON))
-		return;
-#endif
-	/*
-	 * If discovery has not occurred for modes, do not attempt to switch
-	 * to alt mode.
-	 */
-	if (pd_get_svids_discovery(port, TCPCI_MSG_SOP) != PD_DISC_COMPLETE ||
-	    pd_get_modes_discovery(port, TCPCI_MSG_SOP) != PD_DISC_COMPLETE)
-		return;
-
-	if (dp_entry_is_done(port) ||
-	    (IS_ENABLED(CONFIG_USB_PD_TBT_COMPAT_MODE) &&
-	     tbt_entry_is_done(port)) ||
-	    (IS_ENABLED(CONFIG_USB_PD_USB4) && enter_usb_entry_is_done(port))) {
-		dpm_set_mode_entry_done(port);
-		return;
-	}
-
-	/*
-	 * If muxes are still settling, then wait on our next VDM.  We must
-	 * ensure we correctly sequence actions such as USB safe state with TBT
-	 * entry or DP configuration.
-	 */
-	if (IS_ENABLED(CONFIG_USBC_SS_MUX) && !usb_mux_set_completed(port))
-		return;
-
-	if (IS_ENABLED(CONFIG_USB_PD_REQUIRE_AP_MODE_ENTRY) &&
-	    IS_ENABLED(CONFIG_USB_PD_DATA_RESET_MSG) &&
-	    DPM_CHK_FLAG(port, DPM_FLAG_ENTER_ANY) &&
-	    !DPM_CHK_FLAG(port, DPM_FLAG_DATA_RESET_REQUESTED) &&
-	    !DPM_CHK_FLAG(port, DPM_FLAG_DATA_RESET_DONE)) {
-		pd_dpm_request(port, DPM_REQUEST_DATA_RESET);
-		DPM_SET_FLAG(port, DPM_FLAG_DATA_RESET_REQUESTED);
-		return;
-	}
-
-	if (IS_ENABLED(CONFIG_USB_PD_REQUIRE_AP_MODE_ENTRY) &&
-	    IS_ENABLED(CONFIG_USB_PD_DATA_RESET_MSG) &&
-	    !DPM_CHK_FLAG(port, DPM_FLAG_DATA_RESET_DONE)) {
-		return;
-	}
-
-	/* Check if port, port partner and cable support USB4. */
-	if (IS_ENABLED(CONFIG_USB_PD_USB4) && board_is_tbt_usb4_port(port) &&
-	    enter_usb_port_partner_is_capable(port) &&
-	    enter_usb_cable_is_capable(port) &&
-	    dpm_mode_entry_requested(port, TYPEC_MODE_USB4)) {
-		/*
-		 * For certain cables, enter Thunderbolt alt mode with the
-		 * cable and USB4 mode with the port partner.
-		 */
-		if (tbt_cable_entry_required_for_usb4(port)) {
-			vdo_count = ARRAY_SIZE(vdm);
-			status = tbt_setup_next_vdm(port, &vdo_count, vdm,
-						    &tx_type);
-		} else {
-			pd_dpm_request(port, DPM_REQUEST_ENTER_USB);
-			return;
-		}
-	}
-
-	/* If not, check if they support Thunderbolt alt mode. */
-	if (IS_ENABLED(CONFIG_USB_PD_TBT_COMPAT_MODE) &&
-	    board_is_tbt_usb4_port(port) &&
-	    pd_is_mode_discovered_for_svid(port, TCPCI_MSG_SOP,
-					   USB_VID_INTEL) &&
-	    dpm_mode_entry_requested(port, TYPEC_MODE_TBT)) {
-		enter_mode_requested = true;
-		vdo_count = ARRAY_SIZE(vdm);
-		status = tbt_setup_next_vdm(port, &vdo_count, vdm, &tx_type);
-	}
-
-	/* If not, check if they support DisplayPort alt mode. */
-	if (status == MSG_SETUP_UNSUPPORTED &&
-	    !DPM_CHK_FLAG(port, DPM_FLAG_MODE_ENTRY_DONE) &&
-	    pd_is_mode_discovered_for_svid(port, TCPCI_MSG_SOP,
-					   USB_SID_DISPLAYPORT) &&
-	    dpm_mode_entry_requested(port, TYPEC_MODE_DP)) {
-		enter_mode_requested = true;
-		vdo_count = ARRAY_SIZE(vdm);
-		status = dp_setup_next_vdm(port, &vdo_count, vdm);
-	}
-
-	/* Not ready to send a VDM, check again next cycle */
-	if (status == MSG_SETUP_MUX_WAIT)
-		return;
-
-	/*
-	 * If the PE didn't discover any supported (requested) alternate mode,
-	 * just mark setup done and get out of here.
-	 */
-	if (status != MSG_SETUP_SUCCESS &&
-	    !DPM_CHK_FLAG(port, DPM_FLAG_MODE_ENTRY_DONE)) {
-		if (enter_mode_requested) {
-			/*
-			 * TODO(b/168030639): Notify the AP that mode entry
-			 * failed.
-			 */
-			CPRINTS("C%d: No supported alt mode discovered", port);
-		}
-		/*
-		 * If the AP did not request mode entry, it may do so in the
-		 * future, but the DPM is done trying for now.
-		 */
-		dpm_set_mode_entry_done(port);
-		return;
-	}
-
-	if (status != MSG_SETUP_SUCCESS) {
-		dpm_set_mode_entry_done(port);
-		CPRINTS("C%d: Couldn't construct alt mode VDM", port);
-		return;
-	}
-
-	/*
-	 * TODO(b/155890173): Provide a host command to request that the PE send
-	 * an arbitrary VDM via this mechanism.
-	 */
-	if (!pd_setup_vdm_request(port, tx_type, vdm, vdo_count)) {
-		dpm_set_mode_entry_done(port);
-		return;
-	}
-
-	pd_dpm_request(port, DPM_REQUEST_VDM);
-}
-
-static void dpm_attempt_mode_exit(int port)
-{
-	uint32_t vdm[VDO_MAX_SIZE];
-	int vdo_count = ARRAY_SIZE(vdm);
-	enum dpm_msg_setup_status status = MSG_SETUP_ERROR;
-	enum tcpci_msg_type tx_type = TCPCI_MSG_SOP;
-
-	/* First, try Data Reset. If Data Reset completes, all the alt mode
-	 * state checked below will reset to its inactive state. If Data Reset
-	 * is not supported, exit active modes individually.
-	 */
-	if (IS_ENABLED(CONFIG_USB_PD_DATA_RESET_MSG)) {
-		if (!DPM_CHK_FLAG(port, DPM_FLAG_DATA_RESET_REQUESTED) &&
-		    !DPM_CHK_FLAG(port, DPM_FLAG_DATA_RESET_DONE)) {
-			pd_dpm_request(port, DPM_REQUEST_DATA_RESET);
-			DPM_SET_FLAG(port, DPM_FLAG_DATA_RESET_REQUESTED);
-			return;
-		} else if (!DPM_CHK_FLAG(port, DPM_FLAG_DATA_RESET_DONE)) {
-			return;
-		}
-	}
-
-	/* TODO(b/209625351): Data Reset is the only real way to exit from USB4
-	 * mode. If that failed, the TCPM shouldn't try anything else.
-	 */
-	if (IS_ENABLED(CONFIG_USB_PD_USB4) && enter_usb_entry_is_done(port)) {
-		CPRINTS("C%d: USB4 teardown", port);
-		usb4_exit_mode_request(port);
-	}
-
-	/*
-	 * If muxes are still settling, then wait on our next VDM.  We must
-	 * ensure we correctly sequence actions such as USB safe state with TBT
-	 * or DP mode exit.
-	 */
-	if (IS_ENABLED(CONFIG_USBC_SS_MUX) && !usb_mux_set_completed(port))
-		return;
-
-	if (IS_ENABLED(CONFIG_USB_PD_TBT_COMPAT_MODE) && tbt_is_active(port)) {
-		/*
-		 * When the port is in USB4 mode and receives an exit request,
-		 * it leaves USB4 SOP in active state.
-		 */
-		CPRINTS("C%d: TBT teardown", port);
-		tbt_exit_mode_request(port);
-		status = tbt_setup_next_vdm(port, &vdo_count, vdm, &tx_type);
-	} else if (dp_is_active(port)) {
-		CPRINTS("C%d: DP teardown", port);
-		status = dp_setup_next_vdm(port, &vdo_count, vdm);
-	} else {
-		/* Clear exit mode request */
-		dpm_clear_mode_exit_request(port);
-		return;
-	}
-
-	/* This covers error, wait mux, and unsupported cases */
-	if (status != MSG_SETUP_SUCCESS)
-		return;
-
-	if (!pd_setup_vdm_request(port, tx_type, vdm, vdo_count)) {
-		dpm_clear_mode_exit_request(port);
-		return;
-	}
-
-	pd_dpm_request(port, DPM_REQUEST_VDM);
-}
-
 static void dpm_send_req_vdm(int port)
 {
 	/* Set up VDM REQ msg that was passed in previously */
@@ -640,24 +468,6 @@ static void dpm_run_pd_button_sm(int port)
 	DPM_CLR_FLAG(port, DPM_FLAG_PD_BUTTON_RELEASED);
 }
 
-void dpm_run(int port)
-{
-	if (pd_get_data_role(port) == PD_ROLE_DFP) {
-		/* Run DFP related DPM requests */
-		if (DPM_CHK_FLAG(port, DPM_FLAG_EXIT_REQUEST))
-			dpm_attempt_mode_exit(port);
-		else if (!DPM_CHK_FLAG(port, DPM_FLAG_MODE_ENTRY_DONE))
-			dpm_attempt_mode_entry(port);
-
-		/* Run USB PD Power button state machine */
-		dpm_run_pd_button_sm(port);
-	}
-
-	/* Run any VDM REQ messages */
-	if (DPM_CHK_FLAG(port, DPM_FLAG_SEND_VDM_REQ))
-		dpm_send_req_vdm(port);
-}
-
 /*
  * Source-out policy variables and APIs
  *
@@ -1199,3 +1009,375 @@ uint8_t pd_get_bist_share_mode(void)
 {
 	return bist_shared_mode_enabled;
 }
+
+/*
+ * Requests that the PE send one VDM, whichever is next in the mode entry
+ * sequence. This only happens if preconditions for mode entry are met. If
+ * CONFIG_USB_PD_REQUIRE_AP_MODE_ENTRY is enabled, this function waits for the
+ * AP to direct mode entry.
+ *
+ * Returns true when the DPM state is changed in this function.
+ */
+static bool dpm_dfp_enter_mode_msg(int port)
+{
+	int vdo_count = 0;
+	uint32_t vdm[VDO_MAX_SIZE];
+	enum tcpci_msg_type tx_type = TCPCI_MSG_SOP;
+	bool enter_mode_requested =
+		IS_ENABLED(CONFIG_USB_PD_REQUIRE_AP_MODE_ENTRY) ? false : true;
+	enum dpm_msg_setup_status status = MSG_SETUP_UNSUPPORTED;
+
+#ifdef CONFIG_AP_POWER_CONTROL
+	/*
+	 * Do not try to enter mode while CPU is off.
+	 * CPU transitions (e.g b/158634281) can occur during the discovery
+	 * phase or during enter/exit negotiations, and the state
+	 * of the modes can get out of sync, causing the attempt to
+	 * enter the mode to fail prematurely.
+	 */
+	if (!chipset_in_state(CHIPSET_STATE_ANY_SUSPEND | CHIPSET_STATE_ON))
+		return false;
+#endif
+	/*
+	 * If discovery has not occurred for modes, do not attempt to switch
+	 * to alt mode.
+	 */
+	if (pd_get_svids_discovery(port, TCPCI_MSG_SOP) != PD_DISC_COMPLETE ||
+	    pd_get_modes_discovery(port, TCPCI_MSG_SOP) != PD_DISC_COMPLETE)
+		return false;
+
+	if (dp_entry_is_done(port) ||
+	    (IS_ENABLED(CONFIG_USB_PD_TBT_COMPAT_MODE) &&
+	     tbt_entry_is_done(port)) ||
+	    (IS_ENABLED(CONFIG_USB_PD_USB4) && enter_usb_entry_is_done(port))) {
+		dpm_set_mode_entry_done(port);
+		return false;
+	}
+
+	/*
+	 * If AP mode entry is enabled, and a Data Reset has not been done, then
+	 * first request Data Reset prior to attempting to enter any modes.
+	 */
+	if (IS_ENABLED(CONFIG_USB_PD_REQUIRE_AP_MODE_ENTRY) &&
+	    IS_ENABLED(CONFIG_USB_PD_DATA_RESET_MSG) &&
+	    DPM_CHK_FLAG(port, DPM_FLAG_ENTER_ANY) &&
+	    !DPM_CHK_FLAG(port, DPM_FLAG_DATA_RESET_DONE)) {
+		set_state_dpm(port, DPM_DATA_RESET);
+		return true;
+	}
+
+	/* Check if port, port partner and cable support USB4. */
+	if (IS_ENABLED(CONFIG_USB_PD_USB4) && board_is_tbt_usb4_port(port) &&
+	    enter_usb_port_partner_is_capable(port) &&
+	    enter_usb_cable_is_capable(port) &&
+	    dpm_mode_entry_requested(port, TYPEC_MODE_USB4)) {
+		/*
+		 * For certain cables, enter Thunderbolt alt mode with the
+		 * cable and USB4 mode with the port partner.
+		 */
+		if (tbt_cable_entry_required_for_usb4(port)) {
+			vdo_count = ARRAY_SIZE(vdm);
+			status = tbt_setup_next_vdm(port, &vdo_count, vdm,
+						    &tx_type);
+		} else {
+			pd_dpm_request(port, DPM_REQUEST_ENTER_USB);
+			return false;
+		}
+	}
+
+	/* If not, check if they support Thunderbolt alt mode. */
+	if (IS_ENABLED(CONFIG_USB_PD_TBT_COMPAT_MODE) &&
+	    board_is_tbt_usb4_port(port) &&
+	    pd_is_mode_discovered_for_svid(port, TCPCI_MSG_SOP,
+					   USB_VID_INTEL) &&
+	    dpm_mode_entry_requested(port, TYPEC_MODE_TBT)) {
+		enter_mode_requested = true;
+		vdo_count = ARRAY_SIZE(vdm);
+		status = tbt_setup_next_vdm(port, &vdo_count, vdm, &tx_type);
+	}
+
+	/* If not, check if they support DisplayPort alt mode. */
+	if (status == MSG_SETUP_UNSUPPORTED &&
+	    !DPM_CHK_FLAG(port, DPM_FLAG_MODE_ENTRY_DONE) &&
+	    pd_is_mode_discovered_for_svid(port, TCPCI_MSG_SOP,
+					   USB_SID_DISPLAYPORT) &&
+	    dpm_mode_entry_requested(port, TYPEC_MODE_DP)) {
+		enter_mode_requested = true;
+		vdo_count = ARRAY_SIZE(vdm);
+		status = dp_setup_next_vdm(port, &vdo_count, vdm);
+	}
+
+	/* Not ready to send a VDM, check again next cycle */
+	if (status == MSG_SETUP_MUX_WAIT)
+		return false;
+
+	/*
+	 * If the PE didn't discover any supported (requested) alternate mode,
+	 * just mark setup done and get out of here.
+	 */
+	if (status != MSG_SETUP_SUCCESS &&
+	    !DPM_CHK_FLAG(port, DPM_FLAG_MODE_ENTRY_DONE)) {
+		if (enter_mode_requested) {
+			/*
+			 * TODO(b/168030639): Notify the AP that mode entry
+			 * failed.
+			 */
+			CPRINTS("C%d: No supported alt mode discovered", port);
+		}
+		/*
+		 * If the AP did not request mode entry, it may do so in the
+		 * future, but the DPM is done trying for now.
+		 */
+		dpm_set_mode_entry_done(port);
+		return false;
+	}
+
+	if (status != MSG_SETUP_SUCCESS) {
+		dpm_set_mode_entry_done(port);
+		CPRINTS("C%d: Couldn't construct alt mode VDM", port);
+		return false;
+	}
+
+	/*
+	 * TODO(b/155890173): Provide a host command to request that the PE send
+	 * an arbitrary VDM via this mechanism.
+	 */
+	if (!pd_setup_vdm_request(port, tx_type, vdm, vdo_count)) {
+		dpm_set_mode_entry_done(port);
+		return false;
+	}
+
+	/* Wait for PE to handle VDM request */
+	pd_dpm_request(port, DPM_REQUEST_VDM);
+	set_state_dpm(port, DPM_WAITING);
+
+	return true;
+}
+
+/*
+ * Checks to see if either USB4 or ALT-DP/TBT modes need to be exited. If the
+ * DPM is requesting the PE to send an exit message, then this function will
+ * return true to indicate that the DPM state has been changed.
+ */
+static bool dpm_dfp_exit_mode_msg(int port)
+{
+	uint32_t vdm[VDO_MAX_SIZE];
+	int vdo_count = ARRAY_SIZE(vdm);
+	enum dpm_msg_setup_status status = MSG_SETUP_ERROR;
+	enum tcpci_msg_type tx_type = TCPCI_MSG_SOP;
+
+	/* First, try Data Reset. If Data Reset completes, all the alt mode
+	 * state checked below will reset to its inactive state. If Data Reset
+	 * is not supported, exit active modes individually.
+	 */
+	if (IS_ENABLED(CONFIG_USB_PD_DATA_RESET_MSG) &&
+	    !DPM_CHK_FLAG(port, DPM_FLAG_DATA_RESET_DONE)) {
+		set_state_dpm(port, DPM_DATA_RESET);
+		return true;
+	}
+
+	/* TODO(b/209625351): Data Reset is the only real way to exit from USB4
+	 * mode. If that failed, the TCPM shouldn't try anything else.
+	 */
+	if (IS_ENABLED(CONFIG_USB_PD_USB4) && enter_usb_entry_is_done(port)) {
+		CPRINTS("C%d: USB4 teardown", port);
+		usb4_exit_mode_request(port);
+	}
+
+	if (IS_ENABLED(CONFIG_USB_PD_TBT_COMPAT_MODE) && tbt_is_active(port)) {
+		/*
+		 * When the port is in USB4 mode and receives an exit request,
+		 * it leaves USB4 SOP in active state.
+		 */
+		CPRINTS("C%d: TBT teardown", port);
+		tbt_exit_mode_request(port);
+		status = tbt_setup_next_vdm(port, &vdo_count, vdm, &tx_type);
+	} else if (dp_is_active(port)) {
+		CPRINTS("C%d: DP teardown", port);
+		status = dp_setup_next_vdm(port, &vdo_count, vdm);
+	} else {
+		/* Clear exit mode request */
+		dpm_clear_mode_exit_request(port);
+		return false;
+	}
+
+	/* This covers error, wait mux, and unsupported cases */
+	if (status != MSG_SETUP_SUCCESS)
+		return false;
+
+	if (!pd_setup_vdm_request(port, tx_type, vdm, vdo_count)) {
+		dpm_clear_mode_exit_request(port);
+		return false;
+	}
+
+	pd_dpm_request(port, DPM_REQUEST_VDM);
+	set_state_dpm(port, DPM_WAITING);
+
+	return true;
+}
+
+void dpm_run(int port, int evt, int en)
+{
+	switch (local_state[port]) {
+	case SM_PAUSED:
+		if (!en)
+			break;
+		__fallthrough;
+	case SM_INIT:
+		dpm_init(port);
+		local_state[port] = SM_RUN;
+		__fallthrough;
+	case SM_RUN:
+		if (!en) {
+			local_state[port] = SM_PAUSED;
+			/*
+			 * While we are paused, exit all states and wait until
+			 * initialized again.
+			 */
+			set_state(port, &dpm[port].ctx, NULL);
+			break;
+		}
+
+		/* Run state machine */
+		run_state(port, &dpm[port].ctx);
+
+		break;
+	}
+}
+
+/*
+ * DPM_WAITING
+ */
+static void dpm_waiting_entry(const int port)
+{
+	DPM_CLR_FLAG(port, DPM_FLAG_PE_READY);
+	print_current_state(port);
+}
+
+static void dpm_waiting_run(const int port)
+{
+	enum pd_data_role dr = pd_get_data_role(port);
+
+	if (DPM_CHK_FLAG(port, DPM_FLAG_PE_READY)) {
+		if (dr == PD_ROLE_UFP) {
+			set_state_dpm(port, DPM_UFP_READY);
+		} else if (dr == PD_ROLE_DFP) {
+			set_state_dpm(port, DPM_DFP_READY);
+		}
+	}
+}
+
+/*
+ * DPM_DFP_READY
+ */
+static void dpm_dfp_ready_entry(const int port)
+{
+	print_current_state(port);
+}
+
+static void dpm_dfp_ready_run(const int port)
+{
+	if (!DPM_CHK_FLAG(port, DPM_FLAG_PE_READY)) {
+		set_state_dpm(port, DPM_WAITING);
+		return;
+	}
+
+	/* Run power button state machine */
+	dpm_run_pd_button_sm(port);
+
+	/*
+	 * If muxes are still settling, then wait on our next VDM.  We must
+	 * ensure we correctly sequence actions such as USB safe state with TBT
+	 * or DP mode exit.
+	 */
+	if (IS_ENABLED(CONFIG_USBC_SS_MUX) && !usb_mux_set_completed(port))
+		return;
+
+	/* Run DFP related DPM requests */
+	if (DPM_CHK_FLAG(port, DPM_FLAG_EXIT_REQUEST)) {
+		if (dpm_dfp_exit_mode_msg(port))
+			return;
+	} else if (!DPM_CHK_FLAG(port, DPM_FLAG_MODE_ENTRY_DONE)) {
+		if (dpm_dfp_enter_mode_msg(port))
+			return;
+	}
+
+	/* Run any VDM REQ messages */
+	if (DPM_CHK_FLAG(port, DPM_FLAG_SEND_VDM_REQ)) {
+		dpm_send_req_vdm(port);
+		set_state_dpm(port, DPM_WAITING);
+		return;
+	}
+}
+
+/*
+ * DPM_UFP_READY
+ */
+static void dpm_ufp_ready_entry(const int port)
+{
+	print_current_state(port);
+}
+
+static void dpm_ufp_ready_run(const int port)
+{
+	if (!DPM_CHK_FLAG(port, DPM_FLAG_PE_READY)) {
+		set_state_dpm(port, DPM_WAITING);
+		return;
+	}
+
+	if (DPM_CHK_FLAG(port, DPM_FLAG_ENTER_ANY)) {
+		DPM_CLR_FLAG(port, DPM_FLAG_ENTER_DP | DPM_FLAG_ENTER_TBT |
+					   DPM_FLAG_ENTER_USB4);
+		/*
+		 * TODO(b/168030639): Notify the AP that the
+		 * enter mode request failed.
+		 */
+		return;
+	}
+
+	/* Run any VDM REQ messages */
+	if (DPM_CHK_FLAG(port, DPM_FLAG_SEND_VDM_REQ)) {
+		dpm_send_req_vdm(port);
+		set_state_dpm(port, DPM_WAITING);
+		return;
+	}
+}
+
+/*
+ * DPM_DATA_RESET
+ */
+static void dpm_data_reset_entry(const int port)
+{
+	print_current_state(port);
+
+	pd_dpm_request(port, DPM_REQUEST_DATA_RESET);
+}
+
+static void dpm_data_reset_run(const int port)
+{
+	/* Wait for Data Reset to Complete */
+	if (!DPM_CHK_FLAG(port, DPM_FLAG_DATA_RESET_DONE))
+		return;
+
+	set_state_dpm(port, DPM_DFP_READY);
+}
+
+static __const_data const struct usb_state dpm_states[] = {
+	/* Normal States */
+	[DPM_WAITING] = {
+		.entry = dpm_waiting_entry,
+		.run   = dpm_waiting_run,
+	},
+	[DPM_DFP_READY] = {
+		.entry = dpm_dfp_ready_entry,
+		.run   = dpm_dfp_ready_run,
+	},
+	[DPM_UFP_READY] = {
+		.entry = dpm_ufp_ready_entry,
+		.run   = dpm_ufp_ready_run,
+	},
+	[DPM_DATA_RESET] = {
+		.entry = dpm_data_reset_entry,
+		.run   = dpm_data_reset_run,
+	},
+};
diff --git a/common/usbc/usb_pd_host.c b/common/usbc/usb_pd_host.c
index 1e2965d114..0b3958cd47 100644
--- a/common/usbc/usb_pd_host.c
+++ b/common/usbc/usb_pd_host.c
@@ -5,17 +5,17 @@
  * Host commands for TCPMv2 USB PD module
  */
 
-#include <string.h>
-
 #include "console.h"
 #include "ec_commands.h"
 #include "host_command.h"
 #include "usb_mux.h"
 #include "usb_pd.h"
-#include "usb_pd_dpm.h"
+#include "usb_pd_dpm_sm.h"
 #include "usb_pd_tcpm.h"
 #include "util.h"
 
+#include <string.h>
+
 #define CPRINTF(format, args...) cprintf(CC_USBPD, format, ##args)
 #define CPRINTS(format, args...) cprints(CC_USBPD, format, ##args)
 
@@ -112,6 +112,8 @@ static enum ec_status hc_typec_control(struct host_cmd_handler_args *args)
 {
 	const struct ec_params_typec_control *p = args->params;
 	mux_state_t mode;
+	uint32_t data[VDO_MAX_SIZE];
+	enum tcpci_msg_type tx_type;
 
 	if (p->port >= board_get_usb_pd_port_count())
 		return EC_RES_INVALID_PARAM;
@@ -140,6 +142,34 @@ static enum ec_status hc_typec_control(struct host_cmd_handler_args *args)
 		return EC_RES_SUCCESS;
 	case TYPEC_CONTROL_COMMAND_BIST_SHARE_MODE:
 		return pd_set_bist_share_mode(p->bist_share_mode);
+	case TYPEC_CONTROL_COMMAND_SEND_VDM_REQ:
+		if (!IS_ENABLED(CONFIG_USB_PD_VDM_AP_CONTROL))
+			return EC_RES_INVALID_PARAM;
+
+		if (p->vdm_req_params.vdm_data_objects <= 0 ||
+		    p->vdm_req_params.vdm_data_objects > VDO_MAX_SIZE)
+			return EC_RES_INVALID_PARAM;
+
+		memcpy(data, p->vdm_req_params.vdm_data,
+		       sizeof(uint32_t) * p->vdm_req_params.vdm_data_objects);
+
+		switch (p->vdm_req_params.partner_type) {
+		case TYPEC_PARTNER_SOP:
+			tx_type = TCPCI_MSG_SOP;
+			break;
+		case TYPEC_PARTNER_SOP_PRIME:
+			tx_type = TCPCI_MSG_SOP_PRIME;
+			break;
+		case TYPEC_PARTNER_SOP_PRIME_PRIME:
+			tx_type = TCPCI_MSG_SOP_PRIME_PRIME;
+			break;
+		default:
+			return EC_RES_INVALID_PARAM;
+		}
+
+		return pd_request_vdm(p->port, data,
+				      p->vdm_req_params.vdm_data_objects,
+				      tx_type);
 	default:
 		return EC_RES_INVALID_PARAM;
 	}
diff --git a/common/usbc/usb_pe_ctvpd_sm.c b/common/usbc/usb_pe_ctvpd_sm.c
index dbd32a6b7d..223c40c71d 100644
--- a/common/usbc/usb_pe_ctvpd_sm.c
+++ b/common/usbc/usb_pe_ctvpd_sm.c
@@ -6,15 +6,14 @@
 #include "common.h"
 #include "console.h"
 #include "task.h"
-#include "util.h"
+#include "usb_emsg.h"
 #include "usb_pd.h"
 #include "usb_pd_tcpm.h"
 #include "usb_pe_sm.h"
 #include "usb_prl_sm.h"
-#include "usb_pd_tcpm.h"
-#include "usb_tc_sm.h"
-#include "usb_emsg.h"
 #include "usb_sm.h"
+#include "usb_tc_sm.h"
+#include "util.h"
 
 /* USB Policy Engine Charge-Through VCONN Powered Device module */
 
diff --git a/common/usbc/usb_pe_drp_sm.c b/common/usbc/usb_pe_drp_sm.c
index f6cd95463f..0de098788a 100644
--- a/common/usbc/usb_pe_drp_sm.c
+++ b/common/usbc/usb_pe_drp_sm.c
@@ -21,25 +21,25 @@
 #include "system.h"
 #include "task.h"
 #include "tcpm/tcpm.h"
-#include "util.h"
 #include "usb_charge.h"
 #include "usb_common.h"
 #include "usb_dp_alt_mode.h"
+#include "usb_emsg.h"
 #include "usb_mode.h"
 #include "usb_mux.h"
-#include "usb_pd_dpm.h"
-#include "usb_pd_policy.h"
 #include "usb_pd.h"
+#include "usb_pd_dpm_sm.h"
+#include "usb_pd_policy.h"
 #include "usb_pd_tcpm.h"
 #include "usb_pd_timer.h"
 #include "usb_pe_private.h"
 #include "usb_pe_sm.h"
-#include "usb_tbt_alt_mode.h"
 #include "usb_prl_sm.h"
-#include "usb_tc_sm.h"
-#include "usb_emsg.h"
 #include "usb_sm.h"
+#include "usb_tbt_alt_mode.h"
+#include "usb_tc_sm.h"
 #include "usbc_ppc.h"
+#include "util.h"
 
 /*
  * USB Policy Engine Sink / Source module
@@ -202,91 +202,91 @@ typedef int (*svdm_rsp_func)(int port, uint32_t *payload);
 /* List of all Policy Engine level states */
 enum usb_pe_state {
 	/* Super States */
-	PE_PRS_FRS_SHARED,
-	PE_VDM_SEND_REQUEST,
+	PE_PRS_FRS_SHARED, /* pe-st0 */
+	PE_VDM_SEND_REQUEST, /* pe-st1 */
 
 	/* Normal States */
-	PE_SRC_STARTUP,
-	PE_SRC_DISCOVERY,
-	PE_SRC_SEND_CAPABILITIES,
-	PE_SRC_NEGOTIATE_CAPABILITY,
-	PE_SRC_TRANSITION_SUPPLY,
-	PE_SRC_READY,
-	PE_SRC_DISABLED,
-	PE_SRC_CAPABILITY_RESPONSE,
-	PE_SRC_HARD_RESET,
-	PE_SRC_HARD_RESET_RECEIVED,
-	PE_SRC_TRANSITION_TO_DEFAULT,
-	PE_SNK_STARTUP,
-	PE_SNK_DISCOVERY,
-	PE_SNK_WAIT_FOR_CAPABILITIES,
-	PE_SNK_EVALUATE_CAPABILITY,
-	PE_SNK_SELECT_CAPABILITY,
-	PE_SNK_READY,
-	PE_SNK_HARD_RESET,
-	PE_SNK_TRANSITION_TO_DEFAULT,
-	PE_SNK_GIVE_SINK_CAP,
-	PE_SNK_GET_SOURCE_CAP,
-	PE_SNK_TRANSITION_SINK,
-	PE_SEND_SOFT_RESET,
-	PE_SOFT_RESET,
-	PE_SEND_NOT_SUPPORTED,
-	PE_SRC_PING,
-	PE_DRS_EVALUATE_SWAP,
-	PE_DRS_CHANGE,
-	PE_DRS_SEND_SWAP,
-	PE_PRS_SRC_SNK_EVALUATE_SWAP,
-	PE_PRS_SRC_SNK_TRANSITION_TO_OFF,
-	PE_PRS_SRC_SNK_ASSERT_RD,
-	PE_PRS_SRC_SNK_WAIT_SOURCE_ON,
-	PE_PRS_SRC_SNK_SEND_SWAP,
-	PE_PRS_SNK_SRC_EVALUATE_SWAP,
-	PE_PRS_SNK_SRC_TRANSITION_TO_OFF,
-	PE_PRS_SNK_SRC_ASSERT_RP,
-	PE_PRS_SNK_SRC_SOURCE_ON,
-	PE_PRS_SNK_SRC_SEND_SWAP,
-	PE_VCS_EVALUATE_SWAP,
-	PE_VCS_SEND_SWAP,
-	PE_VCS_WAIT_FOR_VCONN_SWAP,
-	PE_VCS_TURN_ON_VCONN_SWAP,
-	PE_VCS_TURN_OFF_VCONN_SWAP,
-	PE_VCS_SEND_PS_RDY_SWAP,
-	PE_VCS_CBL_SEND_SOFT_RESET,
-	PE_VDM_IDENTITY_REQUEST_CBL,
-	PE_INIT_PORT_VDM_IDENTITY_REQUEST,
-	PE_INIT_VDM_SVIDS_REQUEST,
-	PE_INIT_VDM_MODES_REQUEST,
-	PE_VDM_REQUEST_DPM,
-	PE_VDM_RESPONSE,
-	PE_WAIT_FOR_ERROR_RECOVERY,
-	PE_BIST_TX,
-	PE_DEU_SEND_ENTER_USB,
-	PE_DR_GET_SINK_CAP,
-	PE_DR_SNK_GIVE_SOURCE_CAP,
-	PE_DR_SRC_GET_SOURCE_CAP,
+	PE_SRC_STARTUP, /* pe-st2 */
+	PE_SRC_DISCOVERY, /* pe-st3 */
+	PE_SRC_SEND_CAPABILITIES, /* pe-st4 */
+	PE_SRC_NEGOTIATE_CAPABILITY, /* pe-st5 */
+	PE_SRC_TRANSITION_SUPPLY, /* pe-st6 */
+	PE_SRC_READY, /* pe-st7 */
+	PE_SRC_DISABLED, /* pe-st8 */
+	PE_SRC_CAPABILITY_RESPONSE, /* pe-st9 */
+	PE_SRC_HARD_RESET, /* pe-st10 */
+	PE_SRC_HARD_RESET_RECEIVED, /* pe-st11 */
+	PE_SRC_TRANSITION_TO_DEFAULT, /* pe-st12 */
+	PE_SNK_STARTUP, /* pe-st13 */
+	PE_SNK_DISCOVERY, /* pe-st14 */
+	PE_SNK_WAIT_FOR_CAPABILITIES, /* pe-st15 */
+	PE_SNK_EVALUATE_CAPABILITY, /* pe-st16 */
+	PE_SNK_SELECT_CAPABILITY, /* pe-st17 */
+	PE_SNK_READY, /* pe-st18 */
+	PE_SNK_HARD_RESET, /* pe-st19 */
+	PE_SNK_TRANSITION_TO_DEFAULT, /* pe-st20 */
+	PE_SNK_GIVE_SINK_CAP, /* pe-st21 */
+	PE_SNK_GET_SOURCE_CAP, /* pe-st22 */
+	PE_SNK_TRANSITION_SINK, /* pe-st23 */
+	PE_SEND_SOFT_RESET, /* pe-st24 */
+	PE_SOFT_RESET, /* pe-st25 */
+	PE_SEND_NOT_SUPPORTED, /* pe-st26 */
+	PE_SRC_PING, /* pe-st27 */
+	PE_DRS_EVALUATE_SWAP, /* pe-st28 */
+	PE_DRS_CHANGE, /* pe-st29 */
+	PE_DRS_SEND_SWAP, /* pe-st30 */
+	PE_PRS_SRC_SNK_EVALUATE_SWAP, /* pe-st31 */
+	PE_PRS_SRC_SNK_TRANSITION_TO_OFF, /* pe-st32 */
+	PE_PRS_SRC_SNK_ASSERT_RD, /* pe-st33 */
+	PE_PRS_SRC_SNK_WAIT_SOURCE_ON, /* pe-st34 */
+	PE_PRS_SRC_SNK_SEND_SWAP, /* pe-st35 */
+	PE_PRS_SNK_SRC_EVALUATE_SWAP, /* pe-st36 */
+	PE_PRS_SNK_SRC_TRANSITION_TO_OFF, /* pe-st37 */
+	PE_PRS_SNK_SRC_ASSERT_RP, /* pe-st38 */
+	PE_PRS_SNK_SRC_SOURCE_ON, /* pe-st39 */
+	PE_PRS_SNK_SRC_SEND_SWAP, /* pe-st40 */
+	PE_VCS_EVALUATE_SWAP, /* pe-st41 */
+	PE_VCS_SEND_SWAP, /* pe-st42 */
+	PE_VCS_WAIT_FOR_VCONN_SWAP, /* pe-st43 */
+	PE_VCS_TURN_ON_VCONN_SWAP, /* pe-st44 */
+	PE_VCS_TURN_OFF_VCONN_SWAP, /* pe-st45 */
+	PE_VCS_SEND_PS_RDY_SWAP, /* pe-st46 */
+	PE_VCS_CBL_SEND_SOFT_RESET, /* pe-st47 */
+	PE_VDM_IDENTITY_REQUEST_CBL, /* pe-st48 */
+	PE_INIT_PORT_VDM_IDENTITY_REQUEST, /* pe-st49 */
+	PE_INIT_VDM_SVIDS_REQUEST, /* pe-st50 */
+	PE_INIT_VDM_MODES_REQUEST, /* pe-st51 */
+	PE_VDM_REQUEST_DPM, /* pe-st52 */
+	PE_VDM_RESPONSE, /* pe-st53 */
+	PE_WAIT_FOR_ERROR_RECOVERY, /* pe-st54 */
+	PE_BIST_TX, /* pe-st55 */
+	PE_DEU_SEND_ENTER_USB, /* pe-st56 */
+	PE_DR_GET_SINK_CAP, /* pe-st57 */
+	PE_DR_SNK_GIVE_SOURCE_CAP, /* pe-st58 */
+	PE_DR_SRC_GET_SOURCE_CAP, /* pe-st59 */
 
 	/* PD3.0 only states below here*/
 	/* UFP Data Reset States */
-	PE_UDR_SEND_DATA_RESET,
-	PE_UDR_DATA_RESET_RECEIVED,
-	PE_UDR_TURN_OFF_VCONN,
-	PE_UDR_SEND_PS_RDY,
-	PE_UDR_WAIT_FOR_DATA_RESET_COMPLETE,
+	PE_UDR_SEND_DATA_RESET, /* pe-st60 */
+	PE_UDR_DATA_RESET_RECEIVED, /* pe-st61 */
+	PE_UDR_TURN_OFF_VCONN, /* pe-st62 */
+	PE_UDR_SEND_PS_RDY, /* pe-st63 */
+	PE_UDR_WAIT_FOR_DATA_RESET_COMPLETE, /* pe-st64 */
 	/* DFP Data Reset States */
-	PE_DDR_SEND_DATA_RESET,
-	PE_DDR_DATA_RESET_RECEIVED,
-	PE_DDR_WAIT_FOR_VCONN_OFF,
-	PE_DDR_PERFORM_DATA_RESET,
-	PE_FRS_SNK_SRC_START_AMS,
-	PE_GIVE_BATTERY_CAP,
-	PE_GIVE_BATTERY_STATUS,
-	PE_GIVE_STATUS,
-	PE_SEND_ALERT,
-	PE_ALERT_RECEIVED,
-	PE_SRC_CHUNK_RECEIVED,
-	PE_SNK_CHUNK_RECEIVED,
-	PE_VCS_FORCE_VCONN,
-	PE_GET_REVISION,
+	PE_DDR_SEND_DATA_RESET, /* pe-st65 */
+	PE_DDR_DATA_RESET_RECEIVED, /* pe-st66 */
+	PE_DDR_WAIT_FOR_VCONN_OFF, /* pe-st67 */
+	PE_DDR_PERFORM_DATA_RESET, /* pe-st68 */
+	PE_FRS_SNK_SRC_START_AMS, /* pe-st69 */
+	PE_GIVE_BATTERY_CAP, /* pe-st70 */
+	PE_GIVE_BATTERY_STATUS, /* pe-st71 */
+	PE_GIVE_STATUS, /* pe-st72 */
+	PE_SEND_ALERT, /* pe-st73 */
+	PE_ALERT_RECEIVED, /* pe-st74 */
+	PE_SRC_CHUNK_RECEIVED, /* pe-st75 */
+	PE_SNK_CHUNK_RECEIVED, /* pe-st76 */
+	PE_VCS_FORCE_VCONN, /* pe-st77 */
+	PE_GET_REVISION, /* pe-st78 */
 };
 
 /*
@@ -2927,11 +2927,17 @@ static void pe_src_ready_run(int port)
 		if (pe_attempt_port_discovery(port))
 			return;
 
-		/* No DPM requests; attempt mode entry/exit if needed */
-		dpm_run(port);
+		/* Inform DPM state machine that PE is set for messages */
+		dpm_set_pe_ready(port, true);
 	}
 }
 
+static void pe_src_ready_exit(int port)
+{
+	/* Inform DPM state machine that PE is in ready state */
+	dpm_set_pe_ready(port, false);
+}
+
 /**
  * PE_SRC_Disabled
  */
@@ -3803,11 +3809,17 @@ static void pe_snk_ready_run(int port)
 		if (pe_attempt_port_discovery(port))
 			return;
 
-		/* No DPM requests; attempt mode entry/exit if needed */
-		dpm_run(port);
+		/* Inform DPM state machine that PE is set for messages */
+		dpm_set_pe_ready(port, true);
 	}
 }
 
+static void pe_snk_ready_exit(int port)
+{
+	/* Inform DPM state machine that PE is in ready state */
+	dpm_set_pe_ready(port, false);
+}
+
 /**
  * PE_SNK_Hard_Reset
  */
@@ -6502,7 +6514,7 @@ static void pe_vcs_evaluate_swap_entry(int port)
 	 */
 
 	/* DPM rejects a VCONN Swap and port is not a VCONN source*/
-	if (!tc_check_vconn_swap(port) && tc_is_vconn_src(port) < 1) {
+	if (!tc_check_vconn_swap(port) || tc_is_vconn_src(port) < 1) {
 		/* NOTE: PE_VCS_Reject_Swap State embedded here */
 		send_ctrl_msg(port, TCPCI_MSG_SOP, PD_CTRL_REJECT);
 	}
@@ -7674,8 +7686,7 @@ static void pe_ddr_perform_data_reset_run(int port)
 		 * interpretations are mutually exclusive. Resolve that
 		 * ambiguity and update this implementation.
 		 */
-		usb_mux_set(port, USB_PD_MUX_NONE, USB_SWITCH_DISCONNECT,
-			    polarity_rm_dts(pd_get_polarity(port)));
+		set_usb_mux_with_current_data_role(port);
 	} else if (IS_ENABLED(CONFIG_USBC_VCONN) &&
 		   PE_CHK_FLAG(port, PE_FLAGS_VCONN_SWAP_COMPLETE) &&
 		   tc_is_vconn_src(port)) {
@@ -7902,6 +7913,7 @@ static __const_data const struct usb_state pe_states[] = {
 	[PE_SRC_READY] = {
 		.entry = pe_src_ready_entry,
 		.run   = pe_src_ready_run,
+		.exit   = pe_src_ready_exit,
 	},
 	[PE_SRC_DISABLED] = {
 		.entry = pe_src_disabled_entry,
@@ -7948,6 +7960,7 @@ static __const_data const struct usb_state pe_states[] = {
 	[PE_SNK_READY] = {
 		.entry = pe_snk_ready_entry,
 		.run   = pe_snk_ready_run,
+		.exit   = pe_snk_ready_exit,
 	},
 	[PE_SNK_HARD_RESET] = {
 		.entry = pe_snk_hard_reset_entry,
diff --git a/common/usbc/usb_prl_sm.c b/common/usbc/usb_prl_sm.c
index db9f460e61..5c17459834 100644
--- a/common/usbc/usb_prl_sm.c
+++ b/common/usbc/usb_prl_sm.c
@@ -20,16 +20,16 @@
 #include "system.h"
 #include "task.h"
 #include "tcpm/tcpm.h"
-#include "util.h"
 #include "usb_charge.h"
+#include "usb_emsg.h"
 #include "usb_mux.h"
 #include "usb_pd.h"
 #include "usb_pd_timer.h"
 #include "usb_pe_sm.h"
 #include "usb_prl_sm.h"
-#include "usb_tc_sm.h"
-#include "usb_emsg.h"
 #include "usb_sm.h"
+#include "usb_tc_sm.h"
+#include "util.h"
 #include "vpd_api.h"
 
 #ifdef CONFIG_COMMON_RUNTIME
diff --git a/common/usbc/usb_retimer_fw_update.c b/common/usbc/usb_retimer_fw_update.c
index 9b581ea872..a8cabf00a2 100644
--- a/common/usbc/usb_retimer_fw_update.c
+++ b/common/usbc/usb_retimer_fw_update.c
@@ -3,9 +3,6 @@
  * found in the LICENSE file.
  */
 
-#include <stdbool.h>
-#include <stdint.h>
-
 #include "builtin/assert.h"
 #include "compile_time_macros.h"
 #include "console.h"
@@ -15,6 +12,9 @@
 #include "usb_mux.h"
 #include "usb_tc_sm.h"
 
+#include <stdbool.h>
+#include <stdint.h>
+
 #ifdef CONFIG_COMMON_RUNTIME
 #define CPRINTS(format, args...) cprints(CC_USBPD, format, ##args)
 #define CPRINTF(format, args...) cprintf(CC_USBPD, format, ##args)
diff --git a/common/usbc/usb_tc_drp_acc_trysrc_sm.c b/common/usbc/usb_tc_drp_acc_trysrc_sm.c
index ae84574b40..168490d67e 100644
--- a/common/usbc/usb_tc_drp_acc_trysrc_sm.c
+++ b/common/usbc/usb_tc_drp_acc_trysrc_sm.c
@@ -17,7 +17,7 @@
 #include "usb_common.h"
 #include "usb_mux.h"
 #include "usb_pd.h"
-#include "usb_pd_dpm.h"
+#include "usb_pd_dpm_sm.h"
 #include "usb_pd_tcpm.h"
 #include "usb_pd_timer.h"
 #include "usb_pe_sm.h"
@@ -3749,6 +3749,13 @@ __maybe_unused static void tc_ct_attached_snk_entry(int port)
 
 	/* The port shall reject a VCONN swap request. */
 	TC_SET_FLAG(port, TC_FLAGS_REJECT_VCONN_SWAP);
+
+	/*
+	 * Type-C r 2.2: The Host shall not advertise dual-role data or
+	 * dual-role power in its SourceCapability or SinkCapability messages -
+	 * Host changes its advertised capabilities to UFP role/sink only role.
+	 */
+	tc_set_data_role(port, PD_ROLE_UFP);
 }
 
 __maybe_unused static void tc_ct_attached_snk_run(int port)
diff --git a/common/usbc/usb_tc_vpd_sm.c b/common/usbc/usb_tc_vpd_sm.c
index 40b855db9b..1a820c4003 100644
--- a/common/usbc/usb_tc_vpd_sm.c
+++ b/common/usbc/usb_tc_vpd_sm.c
@@ -11,8 +11,8 @@
 #include "tcpm/tcpm.h"
 #include "typec_control.h"
 #include "usb_pd.h"
-#include "usb_tc_sm.h"
 #include "usb_sm.h"
+#include "usb_tc_sm.h"
 #include "vpd_api.h"
 
 /* USB Type-C VCONN Powered Device module */
diff --git a/common/usbc/usbc_task.c b/common/usbc/usbc_task.c
index 6384aae879..5d68221b3e 100644
--- a/common/usbc/usbc_task.c
+++ b/common/usbc/usbc_task.c
@@ -19,19 +19,19 @@
 #include "registers.h"
 #include "system.h"
 #include "task.h"
+#include "tcpm/tcpm.h"
 #include "timer.h"
-#include "util.h"
 #include "usb_charge.h"
 #include "usb_mux.h"
 #include "usb_pd.h"
+#include "usb_pd_dpm_sm.h"
 #include "usb_pd_timer.h"
-#include "usb_prl_sm.h"
-#include "tcpm/tcpm.h"
 #include "usb_pe_sm.h"
 #include "usb_prl_sm.h"
 #include "usb_sm.h"
 #include "usb_tc_sm.h"
 #include "usbc_ppc.h"
+#include "util.h"
 
 #define USBC_EVENT_TIMEOUT (5 * MSEC)
 #define USBC_MIN_EVENT_TIMEOUT (1 * MSEC)
@@ -141,6 +141,10 @@ static bool pd_task_loop(int port)
 	if (IS_ENABLED(CONFIG_USB_PD_TCPC))
 		tcpc_run(port, evt);
 
+	/* Run Device Policy Manager */
+	if (IS_ENABLED(CONFIG_USB_DPM_SM))
+		dpm_run(port, evt, tc_get_pd_enabled(port));
+
 	/* Run policy engine state machine */
 	if (IS_ENABLED(CONFIG_USB_PE_SM))
 		pe_run(port, evt, tc_get_pd_enabled(port));
diff --git a/common/usbc_intr_task.c b/common/usbc_intr_task.c
index 6c724c1279..d1d25c6b68 100644
--- a/common/usbc_intr_task.c
+++ b/common/usbc_intr_task.c
@@ -5,8 +5,6 @@
 
 /* High-priority interrupt tasks implementations */
 
-#include <stdint.h>
-
 #include "builtin/assert.h"
 #include "common.h"
 #include "compile_time_macros.h"
@@ -18,6 +16,8 @@
 #include "usb_pd.h"
 #include "usb_pd_tcpm.h"
 
+#include <stdint.h>
+
 #define CPRINTF(format, args...) cprintf(CC_USBPD, format, ##args)
 #define CPRINTS(format, args...) cprints(CC_USBPD, format, ##args)
 
diff --git a/common/usbc_ocp.c b/common/usbc_ocp.c
index ba975b6105..406deb2b20 100644
--- a/common/usbc_ocp.c
+++ b/common/usbc_ocp.c
@@ -15,13 +15,8 @@
 #include "usbc_ocp.h"
 #include "util.h"
 
-#ifndef TEST_BUILD
 #define CPRINTF(format, args...) cprintf(CC_USBPD, format, ##args)
 #define CPRINTS(format, args...) cprints(CC_USBPD, format, ##args)
-#else
-#define CPRINTF(args...)
-#define CPRINTS(args...)
-#endif
 
 /*
  * Number of seconds until a latched-off port is re-enabled for sourcing after
diff --git a/common/vboot/efs2.c b/common/vboot/efs2.c
index b45109029d..3346a2c76b 100644
--- a/common/vboot/efs2.c
+++ b/common/vboot/efs2.c
@@ -21,8 +21,8 @@
 #include "sha256.h"
 #include "system.h"
 #include "task.h"
-#include "usb_pd.h"
 #include "uart.h"
+#include "usb_pd.h"
 #include "vboot.h"
 #include "vboot_hash.h"
 
diff --git a/common/vboot/vboot.c b/common/vboot/vboot.c
index cf449da1b8..162b396ba5 100644
--- a/common/vboot/vboot.c
+++ b/common/vboot/vboot.c
@@ -17,13 +17,13 @@
 #include "host_command.h"
 #include "rsa.h"
 #include "rwsig.h"
-#include "stdbool.h"
 #include "sha256.h"
 #include "shared_mem.h"
+#include "stdbool.h"
 #include "system.h"
 #include "usb_pd.h"
-#include "vboot.h"
 #include "vb21_struct.h"
+#include "vboot.h"
 
 #define CPRINTS(format, args...) cprints(CC_VBOOT, "VB " format, ##args)
 #define CPRINTF(format, args...) cprintf(CC_VBOOT, "VB " format, ##args)
diff --git a/common/vec3.c b/common/vec3.c
index 67a6049dea..4cf26ebf4c 100644
--- a/common/vec3.c
+++ b/common/vec3.c
@@ -6,8 +6,8 @@
 #include "common.h"
 #include "math.h"
 #include "math_util.h"
-#include "vec3.h"
 #include "util.h"
+#include "vec3.h"
 
 static fpv3_t zero_initialized_vector = { FLOAT_TO_FP(0.0f), FLOAT_TO_FP(0.0f),
 					  FLOAT_TO_FP(0.0f) };
diff --git a/common/version.c b/common/version.c
index 8b1ac5e53d..fe082abbe0 100644
--- a/common/version.c
+++ b/common/version.c
@@ -5,7 +5,6 @@
 
 /* Embed firmware version number in the binary */
 
-#include <stdint.h>
 #include "common.h"
 #include "compile_time_macros.h"
 #include "cros_version.h"
@@ -14,6 +13,8 @@
 #include "stddef.h"
 #include "system.h"
 
+#include <stdint.h>
+
 BUILD_ASSERT(CONFIG_ROLLBACK_VERSION >= 0);
 BUILD_ASSERT(CONFIG_ROLLBACK_VERSION <= INT32_MAX);
 
diff --git a/common/virtual_battery.c b/common/virtual_battery.c
index adb4e96d7f..743f2a8801 100644
--- a/common/virtual_battery.c
+++ b/common/virtual_battery.c
@@ -103,9 +103,6 @@ int virtual_battery_handler(struct ec_response_i2c_passthru *resp, int in_len,
 		if (cache_hit)
 			*err_code = 0;
 		break;
-	default:
-		reset_parse_state();
-		return EC_ERROR_INVAL;
 	}
 
 	acc_write_len += write_len;
@@ -129,10 +126,15 @@ int virtual_battery_handler(struct ec_response_i2c_passthru *resp, int in_len,
 							  read_len, 0);
 			}
 			break;
+		/* LCOV_EXCL_START - Unreachable in IDLE state and remaining
+		 * states covered above.
+		 */
 		default:
 			reset_parse_state();
 			return EC_ERROR_INVAL;
 		}
+		/* LCOV_EXCL_STOP */
+
 		/* Reset the state in the end of messages */
 		reset_parse_state();
 	}
author	YH Lin <yueherngl@google.com>	2022-11-30 21:45:30 +0000
committer	YH Lin <yueherngl@google.com>	2022-11-30 21:45:30 +0000
commit	184d13e77614be3be5374d3fef9d1edf66ec8687 (patch)
tree	d0c89ae8814c2ba35c238c6c0644ec6b1602c63a /common
parent	19d4d68ffa8b6910d716ab5e1953c41b58614a57 (diff)
parent	aa40b859b3a73e5a205bc561c1a29eff38485461 (diff)
download	chrome-ec-184d13e77614be3be5374d3fef9d1edf66ec8687.tar.gz