charge_manager: Enter safe mode at boot

Charge port / current selection often needs to be significantly altered
when a battery cannot provide sufficient charge, so have charge_manager
initially enter safe mode. After a battery with sufficient capacity has
been identified, charge manager will leave safe mode, and port / current
selection will return to standard rules.

BUG=chromium:777596
BRANCH=None
TEST=Pass charge_manager unit tests. On kevin, remove battery, attach
Apple PD charger, verify safe mode is not exited and device does not
brown out. Hot-plug battery and verify safe mode is exited. Next,
remove battery, attach to Samus, verify safe mode is not exited and
device doesn't brown out. Hot-plug battery, verify that safe mode is
exited and no active charge port, due to dual-role exclusion.

Change-Id: I7784865750087a037aad8dbbac058b22c77ba6d4
Signed-off-by: Shawn Nematbakhsh <shawnn@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/733954
Commit-Ready: Shawn N <shawnn@chromium.org>
Tested-by: Shawn N <shawnn@chromium.org>
Reviewed-by: Vincent Palatin <vpalatin@chromium.org>

23 files changed, 120 insertions, 258 deletions

diff --git a/board/coral/board.c b/board/coral/board.c
index 650b89d317..a862541c1b 100644
--- a/board/coral/board.c
+++ b/board/coral/board.c
@@ -593,17 +593,6 @@ int board_set_active_charge_port(int charge_port)
 {
 	enum bd9995x_charge_port bd9995x_port;
 	int bd9995x_port_select = 1;
-	static int initialized;
-
-	/*
-	 * Reject charge port disable if our battery is critical and we
-	 * have yet to initialize a charge port - continue to charge using
-	 * charger ROM / POR settings.
-	 */
-	if (!initialized &&
-	    charge_port == CHARGE_PORT_NONE &&
-	    charge_get_percent() < 2)
-		return -1;
 
 	switch (charge_port) {
 	case USB_PD_PORT_ANX74XX:
@@ -632,7 +621,6 @@ int board_set_active_charge_port(int charge_port)
 	}
 
 	CPRINTS("New chg p%d", charge_port);
-	initialized = 1;
 
 	return bd9995x_select_input_port(bd9995x_port, bd9995x_port_select);
 }
diff --git a/board/elm/board.c b/board/elm/board.c
index 6d7d9746bb..701f61ac6c 100644
--- a/board/elm/board.c
+++ b/board/elm/board.c
@@ -289,17 +289,6 @@ int board_set_active_charge_port(int charge_port)
 			    charge_port < CONFIG_USB_PD_PORT_COUNT);
 	/* check if we are source VBUS on the port */
 	int source = gpio_get_level(GPIO_USB_C0_5V_EN);
-	static int initialized;
-
-	/*
-	 * Reject charge port disable if our battery is critical and we
-	 * have yet to initialize a charge port - continue to charge using
-	 * charger ROM / POR settings.
-	 */
-	if (!initialized &&
-	    charge_port == CHARGE_PORT_NONE &&
-	    charge_get_percent() < 2)
-		return -1;
 
 	if (is_real_port && source) {
 		CPRINTF("Skip enable p%d", charge_port);
@@ -316,7 +305,6 @@ int board_set_active_charge_port(int charge_port)
 		gpio_set_level(GPIO_USB_C0_CHARGE_L, 0);
 	}
 
-	initialized = 1;
 	return EC_SUCCESS;
 }
 
diff --git a/board/eve/board.c b/board/eve/board.c
index ca5b43d63c..d6b4d87c38 100644
--- a/board/eve/board.c
+++ b/board/eve/board.c
@@ -472,17 +472,6 @@ int board_set_active_charge_port(int charge_port)
 {
 	enum bd9995x_charge_port bd9995x_port;
 	int bd9995x_port_select = 1;
-	static int initialized;
-
-	/*
-	 * Reject charge port disable if our battery is critical and we
-	 * have yet to initialize a charge port - continue to charge using
-	 * charger ROM / POR settings.
-	 */
-	if (!initialized &&
-	    charge_port == CHARGE_PORT_NONE &&
-	    charge_get_percent() < 2)
-		return -1;
 
 	switch (charge_port) {
 	case 0:
@@ -511,7 +500,6 @@ int board_set_active_charge_port(int charge_port)
 	}
 
 	CPRINTS("New chg p%d", charge_port);
-	initialized = 1;
 
 	return bd9995x_select_input_port(bd9995x_port, bd9995x_port_select);
 }
diff --git a/board/fizz/board.h b/board/fizz/board.h
index a1203c140a..1e79eb97a1 100644
--- a/board/fizz/board.h
+++ b/board/fizz/board.h
@@ -64,6 +64,7 @@
 
 /* Charger */
 #define CONFIG_CHARGE_MANAGER
+#undef  CONFIG_CHARGE_MANAGER_SAFE_MODE
 
 #define CONFIG_CHARGER_LIMIT_POWER_THRESH_CHG_MW 50000
 
diff --git a/board/grunt/board.c b/board/grunt/board.c
index cd2ebcd215..9a4b341ee7 100644
--- a/board/grunt/board.c
+++ b/board/grunt/board.c
@@ -356,18 +356,6 @@ int pd_snk_is_vbus_provided(int port)
  */
 int board_set_active_charge_port(int charge_port)
 {
-	static int initialized;
-
-	/*
-	 * Reject charge port disable if our battery is critical and we
-	 * have yet to initialize a charge port - continue to charge using
-	 * charger ROM / POR settings.
-	 */
-	if (!initialized &&
-	    charge_port == CHARGE_PORT_NONE &&
-	    charge_get_percent() < 2)
-		return -1;
-
 	switch (charge_port) {
 	case 0:
 		/* Don't charge from a source port */
@@ -395,7 +383,6 @@ int board_set_active_charge_port(int charge_port)
 	}
 
 	CPRINTS("New chg p%d", charge_port);
-	initialized = 1;
 
 	return EC_SUCCESS;
 }
diff --git a/board/kahlee/board.c b/board/kahlee/board.c
index 3826d4a181..159063aa51 100644
--- a/board/kahlee/board.c
+++ b/board/kahlee/board.c
@@ -356,18 +356,6 @@ int pd_snk_is_vbus_provided(int port)
  */
 int board_set_active_charge_port(int charge_port)
 {
-	static int initialized;
-
-	/*
-	 * Reject charge port disable if our battery is critical and we
-	 * have yet to initialize a charge port - continue to charge using
-	 * charger ROM / POR settings.
-	 */
-	if (!initialized &&
-	    charge_port == CHARGE_PORT_NONE &&
-	    charge_get_percent() < 2)
-		return -1;
-
 	switch (charge_port) {
 	case 0:
 		/* Don't charge from a source port */
@@ -395,7 +383,6 @@ int board_set_active_charge_port(int charge_port)
 	}
 
 	CPRINTS("New chg p%d", charge_port);
-	initialized = 1;
 
 	return EC_SUCCESS;
 }
diff --git a/board/kevin/board.c b/board/kevin/board.c
index c10a93c0bd..41d987597d 100644
--- a/board/kevin/board.c
+++ b/board/kevin/board.c
@@ -208,19 +208,6 @@ int board_set_active_charge_port(int charge_port)
 {
 	enum bd9995x_charge_port bd9995x_port;
 	int bd9995x_port_select = 1;
-	static int initialized;
-
-	/*
-	 * Reject charge port disable if our battery is critical and we
-	 * have yet to initialize a charge port - continue to charge using
-	 * charger ROM / POR settings.
-	 */
-	if (!initialized &&
-	    charge_port == CHARGE_PORT_NONE &&
-	    charge_get_percent() < CONFIG_CHARGER_MIN_BAT_PCT_FOR_POWER_ON) {
-		CPRINTS("Bat critical, don't stop charging");
-		return -1;
-	}
 
 	switch (charge_port) {
 	case 0: case 1:
@@ -240,7 +227,6 @@ int board_set_active_charge_port(int charge_port)
 	}
 
 	CPRINTS("New chg p%d", charge_port);
-	initialized = 1;
 
 	return bd9995x_select_input_port(bd9995x_port, bd9995x_port_select);
 }
@@ -248,17 +234,6 @@ int board_set_active_charge_port(int charge_port)
 void board_set_charge_limit(int port, int supplier, int charge_ma,
 			    int max_ma, int charge_mv)
 {
-	/*
-	 * Ignore lower charge ceiling on PD transition if our battery is
-	 * critical, as we may brownout.
-	 */
-	if (supplier == CHARGE_SUPPLIER_PD &&
-	    charge_ma < 1500 &&
-	    charge_get_percent() < CONFIG_CHARGER_MIN_BAT_PCT_FOR_POWER_ON) {
-		CPRINTS("Using max ilim %d", max_ma);
-		charge_ma = max_ma;
-	}
-
 	charge_set_input_current_limit(MAX(charge_ma,
 			       CONFIG_CHARGER_INPUT_CURRENT), charge_mv);
 }
diff --git a/board/nautilus/board.c b/board/nautilus/board.c
index a8e1a7d630..97ccb674e6 100644
--- a/board/nautilus/board.c
+++ b/board/nautilus/board.c
@@ -708,7 +708,6 @@ DECLARE_HOOK(HOOK_AC_CHANGE, board_extpower, HOOK_PRIO_DEFAULT);
  */
 int board_set_active_charge_port(int charge_port)
 {
-	static uint8_t initialized;
 	/* charge port is a physical port */
 	int is_real_port = (charge_port >= 0 &&
 			    charge_port < CONFIG_USB_PD_PORT_COUNT);
@@ -721,13 +720,6 @@ int board_set_active_charge_port(int charge_port)
 		return EC_ERROR_INVAL;
 	}
 
-	if (!initialized &&
-	    charge_port == CHARGE_PORT_NONE &&
-	    charge_get_percent() < CONFIG_CHARGER_MIN_BAT_PCT_FOR_POWER_ON) {
-		CPRINTS("Bat critical, don't stop charging");
-		return -1;
-	}
-
 	CPRINTF("New chg p%d", charge_port);
 
 	if (charge_port == CHARGE_PORT_NONE) {
@@ -743,7 +735,6 @@ int board_set_active_charge_port(int charge_port)
 					     GPIO_USB_C0_CHARGE_L, 0);
 	}
 
-	initialized = 1;
 	return EC_SUCCESS;
 }
 
diff --git a/board/nefario/board.c b/board/nefario/board.c
index 30657fcc78..9016198162 100644
--- a/board/nefario/board.c
+++ b/board/nefario/board.c
@@ -190,20 +190,6 @@ int board_set_active_charge_port(int charge_port)
 {
 	enum bd9995x_charge_port bd9995x_port;
 	int bd9995x_port_select = 1;
-	static int initialized;
-
-	/*
-	 * Reject charge port disable if our battery is critical and we
-	 * have yet to initialize a charge port - continue to charge using
-	 * charger ROM / POR settings.
-	 */
-	if (!initialized &&
-	    charge_port == CHARGE_PORT_NONE &&
-	    (charge_get_percent() < CONFIG_CHARGER_MIN_BAT_PCT_FOR_POWER_ON ||
-	    battery_get_disconnect_state() == BATTERY_DISCONNECTED)) {
-		CPRINTS("Bat critical, don't stop charging");
-		return -1;
-	}
 
 	CPRINTS("New chg p%d", charge_port);
 
@@ -224,25 +210,12 @@ int board_set_active_charge_port(int charge_port)
 		break;
 	}
 
-	initialized = 1;
 	return bd9995x_select_input_port(bd9995x_port, bd9995x_port_select);
 }
 
 void board_set_charge_limit(int port, int supplier, int charge_ma,
 			    int max_ma, int charge_mv)
 {
-	/*
-	 * Ignore lower charge ceiling on PD transition if our battery is
-	 * critical, as we may brownout.
-	 */
-	if (supplier == CHARGE_SUPPLIER_PD &&
-	    charge_ma < 1500 &&
-	    (charge_get_percent() < CONFIG_CHARGER_MIN_BAT_PCT_FOR_POWER_ON ||
-	    battery_get_disconnect_state() == BATTERY_DISCONNECTED)) {
-		CPRINTS("Using max ilim %d", max_ma);
-		charge_ma = max_ma;
-	}
-
 	charge_set_input_current_limit(
 		MAX(charge_ma, CONFIG_CHARGER_INPUT_CURRENT), charge_mv);
 }
diff --git a/board/poppy/board.c b/board/poppy/board.c
index b80be0b7dd..513f17441c 100644
--- a/board/poppy/board.c
+++ b/board/poppy/board.c
@@ -748,7 +748,6 @@ DECLARE_HOOK(HOOK_AC_CHANGE, board_extpower, HOOK_PRIO_DEFAULT);
  */
 int board_set_active_charge_port(int charge_port)
 {
-	static uint8_t initialized;
 	/* charge port is a physical port */
 	int is_real_port = (charge_port >= 0 &&
 			    charge_port < CONFIG_USB_PD_PORT_COUNT);
@@ -761,13 +760,6 @@ int board_set_active_charge_port(int charge_port)
 		return EC_ERROR_INVAL;
 	}
 
-	if (!initialized &&
-	    charge_port == CHARGE_PORT_NONE &&
-	    charge_get_percent() < CONFIG_CHARGER_MIN_BAT_PCT_FOR_POWER_ON) {
-		CPRINTS("Bat critical, don't stop charging");
-		return -1;
-	}
-
 	CPRINTF("New chg p%d", charge_port);
 
 	if (charge_port == CHARGE_PORT_NONE) {
@@ -783,7 +775,6 @@ int board_set_active_charge_port(int charge_port)
 					     GPIO_USB_C0_CHARGE_L, 0);
 	}
 
-	initialized = 1;
 	return EC_SUCCESS;
 }
 
diff --git a/board/reef/board.c b/board/reef/board.c
index cff56b7657..4431adab25 100644
--- a/board/reef/board.c
+++ b/board/reef/board.c
@@ -585,17 +585,6 @@ int board_set_active_charge_port(int charge_port)
 {
 	enum bd9995x_charge_port bd9995x_port;
 	int bd9995x_port_select = 1;
-	static int initialized;
-
-	/*
-	 * Reject charge port disable if our battery is critical and we
-	 * have yet to initialize a charge port - continue to charge using
-	 * charger ROM / POR settings.
-	 */
-	if (!initialized &&
-	    charge_port == CHARGE_PORT_NONE &&
-	    charge_get_percent() < 2)
-		return -1;
 
 	switch (charge_port) {
 	case USB_PD_PORT_ANX74XX:
@@ -624,7 +613,6 @@ int board_set_active_charge_port(int charge_port)
 	}
 
 	CPRINTS("New chg p%d", charge_port);
-	initialized = 1;
 
 	return bd9995x_select_input_port(bd9995x_port, bd9995x_port_select);
 }
diff --git a/board/reef_it8320/board.c b/board/reef_it8320/board.c
index a7f275a9f4..8256cd1a3e 100644
--- a/board/reef_it8320/board.c
+++ b/board/reef_it8320/board.c
@@ -370,17 +370,6 @@ int board_set_active_charge_port(int charge_port)
 {
 	enum bd9995x_charge_port bd9995x_port = 0;
 	int bd9995x_port_select = 1;
-	static int initialized;
-
-	/*
-	 * Reject charge port disable if our battery is critical and we
-	 * have yet to initialize a charge port - continue to charge using
-	 * charger ROM / POR settings.
-	 */
-	if (!initialized &&
-	    charge_port == CHARGE_PORT_NONE &&
-	    charge_get_percent() < 2)
-		return -1;
 
 	switch (charge_port) {
 	case 0:
@@ -409,7 +398,6 @@ int board_set_active_charge_port(int charge_port)
 	}
 
 	CPRINTS("New chg p%d", charge_port);
-	initialized = 1;
 
 	return bd9995x_select_input_port(bd9995x_port, bd9995x_port_select);
 }
diff --git a/board/rowan/board.c b/board/rowan/board.c
index b3e09679bd..31e0da5cea 100644
--- a/board/rowan/board.c
+++ b/board/rowan/board.c
@@ -303,17 +303,6 @@ int board_set_active_charge_port(int charge_port)
 			    charge_port < CONFIG_USB_PD_PORT_COUNT);
 	/* check if we are source VBUS on the port */
 	int source = gpio_get_level(GPIO_USB_C0_5V_EN);
-	static int initialized;
-
-	/*
-	 * Reject charge port disable if our battery is critical and we
-	 * have yet to initialize a charge port - continue to charge using
-	 * charger ROM / POR settings.
-	 */
-	if (!initialized &&
-	    charge_port == CHARGE_PORT_NONE &&
-	    charge_get_percent() < 2)
-		return -1;
 
 	if (is_real_port && source) {
 		CPRINTF("Skip enable p%d", charge_port);
@@ -330,7 +319,6 @@ int board_set_active_charge_port(int charge_port)
 		gpio_set_level(GPIO_USB_C0_CHARGE_L, 0);
 	}
 
-	initialized = 1;
 	return EC_SUCCESS;
 }
 
diff --git a/board/samus_pd/board.c b/board/samus_pd/board.c
index 4a617ede97..3087b9ce03 100644
--- a/board/samus_pd/board.c
+++ b/board/samus_pd/board.c
@@ -475,6 +475,8 @@ static void board_update_battery_soc(int soc)
 {
 	if (batt_soc != soc) {
 		batt_soc = soc;
+		if (batt_soc >= CONFIG_CHARGE_MANAGER_BAT_PCT_SAFE_MODE_EXIT)
+			charge_manager_leave_safe_mode();
 		board_update_charge_limit(desired_charge_rate_ma);
 		hook_notify(HOOK_BATTERY_SOC_CHANGE);
 	}
diff --git a/board/samus_pd/board.h b/board/samus_pd/board.h
index 4aaae34fcc..300b308dac 100644
--- a/board/samus_pd/board.h
+++ b/board/samus_pd/board.h
@@ -21,6 +21,7 @@
 #define CONFIG_BOARD_PRE_INIT
 #define CONFIG_CHARGE_MANAGER
 #define CONFIG_CHARGE_RAMP_SW
+#undef  CONFIG_CMD_CHARGE_SUPPLIER_INFO
 #undef  CONFIG_CMD_HASH
 #undef  CONFIG_CMD_HCDEBUG
 #undef  CONFIG_CMD_I2C_SCAN
diff --git a/board/scarlet/board.c b/board/scarlet/board.c
index f496abcda9..f0899842ff 100644
--- a/board/scarlet/board.c
+++ b/board/scarlet/board.c
@@ -166,21 +166,6 @@ uint16_t tcpc_get_alert_status(void)
 
 int board_set_active_charge_port(int charge_port)
 {
-	static int initialized;
-
-	/*
-	 * Reject charge port disable if our battery is critical and we
-	 * have yet to initialize a charge port - continue to charge using
-	 * charger ROM / POR settings.
-	 */
-	if (!initialized &&
-	    charge_port == CHARGE_PORT_NONE &&
-	    (charge_get_percent() < CONFIG_CHARGER_MIN_BAT_PCT_FOR_POWER_ON ||
-	    battery_get_disconnect_state() == BATTERY_DISCONNECTED)) {
-		CPRINTS("Bat critical, don't stop charging");
-		return -1;
-	}
-
 	CPRINTS("New chg p%d", charge_port);
 
 	switch (charge_port) {
@@ -198,25 +183,12 @@ int board_set_active_charge_port(int charge_port)
 		break;
 	}
 
-	initialized = 1;
 	return EC_SUCCESS;
 }
 
 void board_set_charge_limit(int port, int supplier, int charge_ma,
 			    int max_ma, int charge_mv)
 {
-	/*
-	 * Ignore lower charge ceiling on PD transition if our battery is
-	 * critical, as we may brownout.
-	 */
-	if (supplier == CHARGE_SUPPLIER_PD &&
-	    charge_ma < 1500 &&
-	    (charge_get_percent() < CONFIG_CHARGER_MIN_BAT_PCT_FOR_POWER_ON ||
-	    battery_get_disconnect_state() == BATTERY_DISCONNECTED)) {
-		CPRINTS("Using max ilim %d", max_ma);
-		charge_ma = max_ma;
-	}
-
 	charge_set_input_current_limit(MAX(charge_ma,
 			       CONFIG_CHARGER_INPUT_CURRENT), charge_mv);
 }
diff --git a/board/servo_v4/board.h b/board/servo_v4/board.h
index 8b81800466..aef30865b6 100644
--- a/board/servo_v4/board.h
+++ b/board/servo_v4/board.h
@@ -91,6 +91,7 @@
 #undef CONFIG_TASK_PROFILING
 
 #define CONFIG_CHARGE_MANAGER
+#undef  CONFIG_CHARGE_MANAGER_SAFE_MODE
 #define CONFIG_USB_POWER_DELIVERY
 #define CONFIG_CMD_PD
 #define CONFIG_USB_PD_DUAL_ROLE
diff --git a/board/zoombini/board.c b/board/zoombini/board.c
index 59aca1962f..61aabdb543 100644
--- a/board/zoombini/board.c
+++ b/board/zoombini/board.c
@@ -256,24 +256,11 @@ int board_set_active_charge_port(int port)
 {
 	int is_real_port = (port >= 0 &&
 			    port < CONFIG_USB_PD_PORT_COUNT);
-	static int initialized;
 	int i;
 
 	if (!is_real_port && port != CHARGE_PORT_NONE)
 		return EC_ERROR_INVAL;
 
-	/*
-	 * Reject charge port none if our battery is critical and we
-	 * have yet to initialize a charge port - continue to charge using
-	 * charger ROM / POR settings.
-	 */
-	if (!initialized &&
-	    port == CHARGE_PORT_NONE &&
-	    charge_get_percent() < CONFIG_CHARGER_MIN_BAT_PCT_FOR_POWER_ON) {
-		CPRINTS("Bat critical, don't stop charging");
-		return EC_ERROR_BUSY;
-	}
-
 	CPRINTS("New chg p%d", port);
 
 	if (port == CHARGE_PORT_NONE) {
@@ -281,7 +268,6 @@ int board_set_active_charge_port(int port)
 		gpio_set_level(GPIO_USB_C0_CHARGE_EN_L, 1);
 		gpio_set_level(GPIO_USB_C1_CHARGE_EN_L, 1);
 		gpio_set_level(GPIO_USB_C2_CHARGE_EN_L, 1);
-		initialized = 1;
 		return EC_SUCCESS;
 	}
 
@@ -297,7 +283,6 @@ int board_set_active_charge_port(int port)
 	gpio_set_level(GPIO_USB_C0_CHARGE_EN_L, port != 0);
 	gpio_set_level(GPIO_USB_C1_CHARGE_EN_L, port != 1);
 	gpio_set_level(GPIO_USB_C2_CHARGE_EN_L, port != 2);
-	initialized = 1;
 
 	/*
 	 * Turn on the PP2 FET such that power actually flows and turn off the
@@ -316,18 +301,6 @@ void board_set_charge_limit(int port, int supplier, int charge_ma,
 			    int max_ma, int charge_mv)
 {
 	/*
-	 * Ignore lower charge ceiling on PD transition if our battery is
-	 * critical, as we may brownout.
-	 */
-	if (supplier == CHARGE_SUPPLIER_PD &&
-	    charge_ma < 1500 &&
-	    charge_get_percent() < CONFIG_CHARGER_MIN_BAT_PCT_FOR_POWER_ON) {
-		CPRINTS("Using max ilim %d", max_ma);
-		charge_ma = max_ma;
-	}
-
-
-	/*
 	 * To protect the charge inductor, at voltages above 18V we should
 	 * set the current limit to 2.7A.
 	 */
diff --git a/common/charge_manager.c b/common/charge_manager.c
index 9a753aa011..a068897620 100644
--- a/common/charge_manager.c
+++ b/common/charge_manager.c
@@ -86,6 +86,28 @@ static volatile uint32_t source_port_bitmap;
 BUILD_ASSERT(sizeof(source_port_bitmap)*8 >= CONFIG_USB_PD_PORT_COUNT);
 static uint8_t source_port_last_rp[CONFIG_USB_PD_PORT_COUNT];
 
+/*
+ * charge_manager initially operates in safe mode until asked to leave (through
+ * charge_manager_leave_safe_mode()). While in safe mode, the following
+ * behavior is altered:
+ *
+ * 1) All chargers are considered dedicated (and thus are valid charge source
+ *    candidates) for the purpose of port selection.
+ * 2) Charge ceilings are ignored. Most significantly, ILIM won't drop on PD
+ *    voltage transition. If current load is high during transition, some
+ *    chargers may brown-out.
+ * 3) CHARGE_PORT_NONE will not be selected (POR default charge port will
+ *    remain selected rather than CHARGE_PORT_NONE).
+ *
+ * After leaving safe mode, charge_manager reverts to its normal behavior and
+ * immediately selects charge port and current using standard rules.
+ */
+#ifdef CONFIG_CHARGE_MANAGER_SAFE_MODE
+static int left_safe_mode;
+#else
+static const int left_safe_mode = 1;
+#endif
+
 enum charge_manager_change_type {
 	CHANGE_CHARGE,
 	CHANGE_DUALROLE,
@@ -112,8 +134,9 @@ static int is_connected(int port)
 		return 1;
 	return pd_is_connected(port);
 }
-#endif
+#endif /* !TEST_BUILD */
 
+#ifndef CONFIG_CHARGE_MANAGER_DRP_CHARGING
 /**
  * In certain cases we need to override the default behavior of not charging
  * from non-dedicated chargers. If the system is in RO and locked, we have no
@@ -125,25 +148,13 @@ static int is_connected(int port)
  * @return	1 when we need to override the a non-dedicated charger
  *		to be a dedicated one, 0 otherwise.
  */
-#ifdef CONFIG_BATTERY
 static int charge_manager_spoof_dualrole_capability(void)
 {
-	int spoof_dualrole =  (system_get_image_copy() == SYSTEM_IMAGE_RO &&
-			       system_is_locked()) ||
-			       (battery_is_present() != BP_YES);
-#ifdef CONFIG_BATTERY_REVIVE_DISCONNECT
-	spoof_dualrole |= (battery_get_disconnect_state() !=
-			   BATTERY_NOT_DISCONNECTED);
-#endif
-	return spoof_dualrole;
-}
-#else /* CONFIG_BATTERY */
-/* No battery, so always charge from input port. */
-static inline int charge_manager_spoof_dualrole_capability(void)
-{
-	return 1;
+	return (system_get_image_copy() == SYSTEM_IMAGE_RO &&
+		system_is_locked()) || !left_safe_mode;
+
 }
-#endif /* CONFIG_BATTERY */
+#endif /* !CONFIG_CHARGE_MANAGER_DRP_CHARGING */
 
 /**
  * Initialize available charge. Run before board init, so board init can
@@ -152,7 +163,6 @@ static inline int charge_manager_spoof_dualrole_capability(void)
 static void charge_manager_init(void)
 {
 	int i, j;
-	int spoof_capability = charge_manager_spoof_dualrole_capability();
 
 	for (i = 0; i < CHARGE_PORT_COUNT; ++i) {
 		for (j = 0; j < CHARGE_SUPPLIER_COUNT; ++j) {
@@ -163,7 +173,7 @@ static void charge_manager_init(void)
 		}
 		for (j = 0; j < CEIL_REQUESTOR_COUNT; ++j)
 			charge_ceil[i][j] = CHARGE_CEIL_NONE;
-		if (spoof_capability || !is_pd_port(i))
+		if (!is_pd_port(i))
 			dualrole_capability[i] = CAP_DEDICATED;
 		if (is_pd_port(i))
 			source_port_last_rp[i] = CONFIG_USB_PD_PULLUP;
@@ -492,7 +502,8 @@ static void charge_manager_get_best_charge_port(int *new_port,
 				 * it is our override port.
 				 */
 				if (dualrole_capability[j] != CAP_DEDICATED &&
-				    override_port != j)
+				    override_port != j &&
+				    !charge_manager_spoof_dualrole_capability())
 					continue;
 #endif
 
@@ -550,6 +561,9 @@ static void charge_manager_refresh(void)
 	while (1) {
 		charge_manager_get_best_charge_port(&new_port, &new_supplier);
 
+		if (!left_safe_mode && new_port == CHARGE_PORT_NONE)
+			return;
+
 		/*
 		 * If the port or supplier changed, make an attempt to switch to
 		 * the port. We will re-set the active port on a supplier change
@@ -563,14 +577,8 @@ static void charge_manager_refresh(void)
 		     board_set_active_charge_port(new_port) == EC_SUCCESS)
 			break;
 
-		/*
-		 * Allow 'Dont charge' request to be rejected only if it
-		 * is our initial selection.
-		 */
-		if (new_port == CHARGE_PORT_NONE) {
-			ASSERT(!active_charge_port_initialized);
-			return;
-		}
+		/* 'Dont charge' request must be accepted. */
+		ASSERT(new_port != CHARGE_PORT_NONE);
 
 		/*
 		 * Zero the available charge on the rejected port so that
@@ -608,7 +616,7 @@ static void charge_manager_refresh(void)
 #endif /* CONFIG_CHARGE_RAMP_HW */
 		/* Enforce port charge ceiling. */
 		ceil = charge_manager_get_ceil(new_port);
-		if (ceil != CHARGE_CEIL_NONE)
+		if (left_safe_mode && ceil != CHARGE_CEIL_NONE)
 			new_charge_current = MIN(ceil,
 						 new_charge_current_uncapped);
 		else
@@ -884,9 +892,6 @@ void charge_manager_update_charge(int supplier,
 
 void charge_manager_update_dualrole(int port, enum dualrole_capabilities cap)
 {
-	if (charge_manager_spoof_dualrole_capability())
-		cap = CAP_DEDICATED;
-
 	/* Ignore when capability is unchanged */
 	if (cap != dualrole_capability[port]) {
 		dualrole_capability[port] = cap;
@@ -894,6 +899,18 @@ void charge_manager_update_dualrole(int port, enum dualrole_capabilities cap)
 	}
 }
 
+#ifdef CONFIG_CHARGE_MANAGER_SAFE_MODE
+void charge_manager_leave_safe_mode(void)
+{
+	if (left_safe_mode)
+		return;
+
+	left_safe_mode = 1;
+	if (charge_manager_is_seeded())
+		hook_call_deferred(&charge_manager_refresh_data, 0);
+}
+#endif
+
 void charge_manager_set_ceil(int port, enum ceil_requestor requestor, int ceil)
 {
 	if (charge_ceil[port][requestor] != ceil) {
@@ -909,7 +926,7 @@ void charge_manager_force_ceil(int port, int ceil)
 	 * Force our input current to ceil if we're exceeding it, without
 	 * waiting for our deferred task to run.
 	 */
-	if (port == charge_port && ceil < charge_current)
+	if (left_safe_mode && port == charge_port && ceil < charge_current)
 		board_set_charge_limit(port, CHARGE_SUPPLIER_PD, ceil,
 				       charge_current_uncapped, charge_voltage);
 
@@ -1178,11 +1195,12 @@ DECLARE_CONSOLE_COMMAND(chglim, command_external_power_limit,
 #ifdef CONFIG_CMD_CHARGE_SUPPLIER_INFO
 static int charge_supplier_info(int argc, char **argv)
 {
-	ccprintf("port=%d, type=%d, cur=%dmA, vtg=%dmV\n",
+	ccprintf("port=%d, type=%d, cur=%dmA, vtg=%dmV, lsm=%d\n",
 			charge_manager_get_active_charge_port(),
 			charge_supplier,
 			charge_current,
-			charge_voltage);
+			charge_voltage,
+			left_safe_mode);
 
 	return 0;
 }
diff --git a/common/charge_state_v2.c b/common/charge_state_v2.c
index df87184813..cb742f1bb6 100644
--- a/common/charge_state_v2.c
+++ b/common/charge_state_v2.c
@@ -875,6 +875,13 @@ wait_for_it:
 		}
 #endif
 
+#ifdef CONFIG_CHARGE_MANAGER
+		if (curr.batt.state_of_charge >=
+		    CONFIG_CHARGE_MANAGER_BAT_PCT_SAFE_MODE_EXIT &&
+		    !battery_seems_to_be_disconnected)
+			charge_manager_leave_safe_mode();
+#endif
+
 		/* Keep the AP informed */
 		if (need_static)
 			need_static = update_static_battery_info();
diff --git a/include/charge_manager.h b/include/charge_manager.h
index ff6788126e..6d5bacc2df 100644
--- a/include/charge_manager.h
+++ b/include/charge_manager.h
@@ -81,6 +81,12 @@ enum dualrole_capabilities {
 void charge_manager_update_dualrole(int port, enum dualrole_capabilities cap);
 
 /**
+ * Tell charge_manager to leave safe mode and switch to standard port / ILIM
+ * selection logic.
+ */
+void charge_manager_leave_safe_mode(void);
+
+/**
  * Charge ceiling can be set independently by different tasks / functions,
  * for different purposes.
  */
diff --git a/include/config.h b/include/config.h
index eb6d3e1d47..6d1e7ee9d1 100644
--- a/include/config.h
+++ b/include/config.h
@@ -434,6 +434,12 @@
 /* Handle the external power limit host command in charge manager */
 #undef CONFIG_CHARGE_MANAGER_EXTERNAL_POWER_LIMIT
 
+/* Initially enter safe mode, with relaxed port / current selection rules */
+#define CONFIG_CHARGE_MANAGER_SAFE_MODE
+
+/* Leave safe mode when battery pct meets or exceeds this value */
+#define CONFIG_CHARGE_MANAGER_BAT_PCT_SAFE_MODE_EXIT 2
+
 /* The hardware has some input current ramping/back-off mechanism */
 #undef CONFIG_CHARGE_RAMP_HW
 
diff --git a/test/charge_manager.c b/test/charge_manager.c
index b40d42611a..90bdf37a66 100644
--- a/test/charge_manager.c
+++ b/test/charge_manager.c
@@ -164,6 +164,48 @@ static int test_initialization(void)
 	return EC_SUCCESS;
 }
 
+static int test_safe_mode(void)
+{
+	int port = 0;
+	struct charge_port_info charge;
+
+	/* Initialize table to no charge */
+	initialize_charge_table(0, 5000, 5000);
+
+	/*
+	 * Set a 2A non-dedicated charger on port 0 and verify that
+	 * it is selected, due to safe mode.
+	 */
+	charge_manager_update_dualrole(port, CAP_DUALROLE);
+	charge.current = 2000;
+	charge.voltage = 5000;
+	charge_manager_update_charge(CHARGE_SUPPLIER_TEST2, port, &charge);
+	wait_for_charge_manager_refresh();
+	TEST_ASSERT(active_charge_port == port);
+	TEST_ASSERT(active_charge_limit == 2000);
+
+	/* Verify ceil is ignored, due to safe mode. */
+	charge_manager_set_ceil(port, 0, 500);
+	wait_for_charge_manager_refresh();
+	TEST_ASSERT(active_charge_limit == 2000);
+
+	/*
+	 * Leave safe mode and verify normal port selection rules go
+	 * into effect.
+	 */
+	charge_manager_leave_safe_mode();
+	wait_for_charge_manager_refresh();
+#ifdef CONFIG_CHARGE_MANAGER_DRP_CHARGING
+	TEST_ASSERT(active_charge_port == port);
+	TEST_ASSERT(active_charge_limit == 500);
+#else
+	TEST_ASSERT(active_charge_port == CHARGE_PORT_NONE);
+#endif
+
+	/* For subsequent tests, safe mode is exited. */
+	return EC_SUCCESS;
+}
+
 static int test_priority(void)
 {
 	struct charge_port_info charge;
@@ -749,6 +791,7 @@ void run_test(void)
 	test_reset();
 
 	RUN_TEST(test_initialization);
+	RUN_TEST(test_safe_mode);
 	RUN_TEST(test_priority);
 	RUN_TEST(test_charge_ceil);
 	RUN_TEST(test_new_power_request);