servo_v4: Support dual-role port

Support a new flag to enable DRP. The do_cc() function becomes having 4
arguments. So change to merge them into a single cc_config with
different bit masks.

For the force_source or force_sink role, we explicitly set the the Rp
or Rd resistors on CC lines. But for DRP role, the Rp/Rd toggling is
controlled by the PD state machine. So don't set any CC pull resistor
for DRP.

Support an "on" option in "cc" command to restore the previous role
before emulating detach.

Add "usbc_action drp" to toggle the DRP state, which is compatible with
Plankton.

BRANCH=servo
BUG=b:135691171
TEST=Ran the firmware_PDTrySrc test passed, and not break all the
existing PD FAFT tests.

Change-Id: I3b90611c9840f502e496c42f80354e7ee002f96e
Signed-off-by: Wai-Hong Tam <waihong@google.com>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1682919
Reviewed-by: Aseda Aboagye <aaboagye@chromium.org>
(cherry picked from commit b1d81401eebb4444123cacac08e88ef6e38ee58d)
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1686279

2 files changed, 135 insertions, 79 deletions

diff --git a/board/servo_v4/usb_pd_config.h b/board/servo_v4/usb_pd_config.h
index e77f4b2227..057cb22e56 100644
--- a/board/servo_v4/usb_pd_config.h
+++ b/board/servo_v4/usb_pd_config.h
@@ -55,6 +55,12 @@
 /* use the hardware accelerator for CRC */
 #define CONFIG_HW_CRC
 
+/* Servo v4 CC configuration */
+#define CC_DETACH	(1 << 0)   /* Emulate detach: both CC open */
+#define CC_DISABLE_DTS	(1 << 1)   /* Apply resistors to single or both CC? */
+#define CC_ALLOW_SRC	(1 << 2)   /* Allow charge through by policy? */
+#define CC_ENABLE_DRP	(1 << 3)   /* Enable dual-role port */
+
 /* TX uses SPI1 on PB3-4 for CHG port, SPI2 on PB 13-14 for DUT port */
 #define SPI_REGS(p) ((p) ? STM32_SPI2_REGS : STM32_SPI1_REGS)
 static inline void spi_enable_clock(int port)
diff --git a/board/servo_v4/usb_pd_policy.c b/board/servo_v4/usb_pd_policy.c
index 9a781c6534..ec127c734f 100644
--- a/board/servo_v4/usb_pd_policy.c
+++ b/board/servo_v4/usb_pd_policy.c
@@ -32,6 +32,28 @@
 
 #define VBUS_UNCHANGED(curr, pend, new) (curr == new && pend == new)
 
+/* Macros to config the PD role */
+#define CONFIG_SET_CLEAR(c, set, clear) ((c | (set)) & ~(clear))
+#define CONFIG_SRC(c) CONFIG_SET_CLEAR(c, \
+				CC_DISABLE_DTS | CC_ALLOW_SRC, \
+				CC_ENABLE_DRP)
+#define CONFIG_SNK(c) CONFIG_SET_CLEAR(c, \
+				CC_DISABLE_DTS, \
+				CC_ALLOW_SRC | CC_ENABLE_DRP)
+#define CONFIG_DRP(c) CONFIG_SET_CLEAR(c, \
+				CC_DISABLE_DTS | CC_ALLOW_SRC | CC_ENABLE_DRP, \
+				0)
+#define CONFIG_SRCDTS(c) CONFIG_SET_CLEAR(c, \
+				CC_ALLOW_SRC, \
+				CC_ENABLE_DRP | CC_DISABLE_DTS)
+#define CONFIG_SNKDTS(c) CONFIG_SET_CLEAR(c, \
+				0, \
+				CC_ALLOW_SRC | CC_ENABLE_DRP | CC_DISABLE_DTS)
+#define CONFIG_DRPDTS(c) CONFIG_SET_CLEAR(c, \
+				CC_ALLOW_SRC | CC_ENABLE_DRP, \
+				CC_DISABLE_DTS)
+
+
 /*
  * Dynamic PDO that reflects capabilities present on the CHG port. Allow for
  * multiple entries so that we can offer greater than 5V charging. The 1st
@@ -61,15 +83,7 @@ static int active_charge_port = CHARGE_PORT_NONE;
 static enum charge_supplier active_charge_supplier;
 static uint8_t vbus_rp = TYPEC_RP_RESERVED;
 
-/* Flag to emulate detach, i.e. making both CC lines open. */
-static int disable_cc;
-/*
- * DTS mode: enabled connects resistors to both CC line to activate cr50,
- * disabled connects to one only as in the standard USBC cable.
- */
-static int disable_dts_mode;
-/* Do we allow charge through by policy? */
-static int allow_src_mode = 1;
+static int cc_config = CC_ALLOW_SRC;
 
 /* Voltage thresholds for no connect in DTS mode */
 static int pd_src_vnc_dts[TYPEC_RP_RESERVED][2] = {
@@ -121,17 +135,35 @@ static int charge_port_is_active(void)
 	return active_charge_port == CHG && vbus[CHG].mv > 0;
 }
 
+static int is_charge_through_allowed(void)
+{
+	return charge_port_is_active() && cc_config & CC_ALLOW_SRC;
+}
+
+static int get_dual_role_of_src(void)
+{
+	return cc_config & CC_ENABLE_DRP ? PD_DRP_TOGGLE_ON :
+					   PD_DRP_FORCE_SOURCE;
+}
+
 static void dut_allow_charge(void)
 {
 	/*
 	 * Update to charge enable if charger still present and not
 	 * already charging.
 	 */
-	if (charge_port_is_active() && allow_src_mode &&
-	    pd_get_dual_role(DUT) != PD_DRP_FORCE_SOURCE) {
+	if (is_charge_through_allowed() &&
+	    pd_get_dual_role(DUT) != PD_DRP_FORCE_SOURCE &&
+	    pd_get_dual_role(DUT) != PD_DRP_TOGGLE_ON) {
 		CPRINTS("Enable DUT charge through");
-		pd_set_dual_role(DUT, PD_DRP_FORCE_SOURCE);
-		pd_set_host_mode(DUT, 1);
+		pd_set_dual_role(DUT, get_dual_role_of_src());
+		/*
+		 * If DRP role, don't set any CC pull resistor, the PD
+		 * state machine will toggle and set the pull resistors
+		 * when needed.
+		 */
+		if (!(cc_config & CC_ENABLE_DRP))
+			pd_set_host_mode(DUT, 1);
 		pd_update_contract(DUT);
 	}
 }
@@ -153,8 +185,8 @@ static void board_manage_dut_port(void)
 	allowed_role = PD_DRP_FORCE_SINK;
 
 	/* If VBUS charge through is available, mark as such. */
-	if (charge_port_is_active() && allow_src_mode)
-		allowed_role = PD_DRP_FORCE_SOURCE;
+	if (is_charge_through_allowed())
+		allowed_role = get_dual_role_of_src();
 
 	current_role = pd_get_dual_role(DUT);
 	if (current_role != allowed_role) {
@@ -283,7 +315,7 @@ int pd_tcpc_cc_nc(int port, int cc_volt, int cc_sel)
 		return 1;
 
 	/* Select the correct voltage threshold for current Rp and DTS mode */
-	if (disable_dts_mode)
+	if (cc_config & CC_DISABLE_DTS)
 		nc = cc_volt >= pd_src_vnc[rp_index];
 	else
 		nc = cc_volt >= pd_src_vnc_dts[rp_index][cc_sel];
@@ -309,7 +341,7 @@ int pd_tcpc_cc_ra(int port, int cc_volt, int cc_sel)
 		return 0;
 
 	/* Select the correct voltage threshold for current Rp and DTS mode */
-	if (disable_dts_mode)
+	if (cc_config & CC_DISABLE_DTS)
 		ra = cc_volt < pd_src_rd_threshold[rp_index];
 	else
 		ra = cc_volt < pd_src_rd_threshold_dts[rp_index][cc_sel];
@@ -323,7 +355,7 @@ int pd_adc_read(int port, int cc)
 
 	if (port == 0)
 		mv = adc_read_channel(cc ? ADC_CHG_CC2_PD : ADC_CHG_CC1_PD);
-	else if (!disable_cc) {
+	else if (!(cc_config & CC_DETACH)) {
 		/*
 		 * In servo v4 hardware logic, both CC lines are wired directly
 		 * to DUT. When servo v4 as a snk, DUT may source Vconn to CC2
@@ -338,15 +370,15 @@ int pd_adc_read(int port, int cc)
 		 * leaves CC2 open. Need change when it supports switching CC
 		 * polarity.
 		 */
-		if (disable_dts_mode && cc_pull_stored == TYPEC_CC_RD &&
-		    port == DUT && cc == 1)
+		if ((cc_config & CC_DISABLE_DTS) &&
+		    cc_pull_stored == TYPEC_CC_RD && port == DUT && cc == 1)
 			mv = 0;
 		else
 			mv = adc_read_channel(cc ? ADC_DUT_CC2_PD :
 						   ADC_DUT_CC1_PD);
 	} else {
 		/*
-		 * When disable_cc, fake the voltage on CC to 0 to avoid
+		 * When emulating detach, fake the voltage on CC to 0 to avoid
 		 * triggering some debounce logic.
 		 *
 		 * The servo v4 makes Rd/Rp open but the DUT may present Rd/Rp
@@ -362,7 +394,7 @@ int pd_adc_read(int port, int cc)
 
 static int board_set_rp(int rp)
 {
-	if (disable_dts_mode) {
+	if (cc_config & CC_DISABLE_DTS) {
 		/*
 		 * DTS mode is disabled, so only present the requested Rp value
 		 * on CC1 and leave all Rp/Rd resistors on CC2 disconnected.
@@ -438,7 +470,7 @@ int pd_set_rp_rd(int port, int cc_pull, int rp_value)
 		return EC_ERROR_UNIMPLEMENTED;
 
 	/* CC is disabled for emulating detach. Don't change Rd/Rp. */
-	if (disable_cc)
+	if (cc_config & CC_DETACH)
 		return EC_SUCCESS;
 
 	/* By default disconnect all Rp/Rd resistors from both CC lines */
@@ -467,7 +499,7 @@ int pd_set_rp_rd(int port, int cc_pull, int rp_value)
 		 * CC1.
 		 */
 		gpio_set_flags(GPIO_USB_DUT_CC1_RD, GPIO_OUT_LOW);
-		if (!disable_dts_mode)
+		if (!(cc_config & CC_DISABLE_DTS))
 			gpio_set_flags(GPIO_USB_DUT_CC2_RD, GPIO_OUT_LOW);
 	}
 
@@ -567,7 +599,7 @@ int pd_set_power_supply_ready(int port)
 
 		vbus[DUT].mv = vbus[CHG].mv;
 		vbus[DUT].ma = vbus[CHG].mv;
-		pd_set_dual_role(DUT, PD_DRP_FORCE_SOURCE);
+		pd_set_dual_role(DUT, get_dual_role_of_src());
 	} else {
 		vbus[DUT].mv = 0;
 		vbus[DUT].ma = 0;
@@ -710,28 +742,26 @@ const int supported_modes_cnt = ARRAY_SIZE(supported_modes);
 static void print_cc_mode(void)
 {
 	/* Get current CCD status */
-	ccprintf("cc: %s\n", disable_cc ? "off" : "on");
-	ccprintf("dts mode: %s\n", disable_dts_mode ? "off" : "on");
+	ccprintf("cc: %s\n", cc_config & CC_DETACH ? "off" : "on");
+	ccprintf("dts mode: %s\n", cc_config & CC_DISABLE_DTS ? "off" : "on");
 	ccprintf("chg mode: %s\n",
-		pd_get_dual_role(DUT) == PD_DRP_FORCE_SOURCE ?
-		"on" : "off");
-	ccprintf("chg allowed: %s\n", allow_src_mode ? "on" : "off");
+		 gpio_get_level(GPIO_DUT_CHG_EN) ? "on" : "off");
+	ccprintf("chg allowed: %s\n", cc_config & CC_ALLOW_SRC ? "on" : "off");
+	ccprintf("drp enabled: %s\n", cc_config & CC_ENABLE_DRP ? "on" : "off");
 }
 
 
-static void do_cc(int disable_cc_new, int disable_dts_new, int allow_src_new)
+static void do_cc(int cc_config_new)
 {
+	int chargeable;
 	int dualrole;
 
-	if ((disable_cc_new != disable_cc) ||
-	    (disable_dts_new != disable_dts_mode) ||
-	    (allow_src_new != allow_src_mode)) {
-		if (!disable_cc) {
+	if (cc_config_new != cc_config) {
+		if (!(cc_config & CC_DETACH)) {
 			/* Force detach */
 			pd_power_supply_reset(DUT);
 			/* Always set to 0 here so both CC lines are changed */
-			disable_dts_mode = 0;
-			allow_src_mode = 0;
+			cc_config &= ~(CC_DISABLE_DTS & CC_ALLOW_SRC);
 
 			/* Remove Rp/Rd on both CC lines */
 			pd_comm_enable(DUT, 0);
@@ -741,36 +771,37 @@ static void do_cc(int disable_cc_new, int disable_dts_new, int allow_src_new)
 			 * If just changing mode (cc keeps enabled), give some
 			 * time for DUT to detach, use tErrorRecovery.
 			 */
-			if (!disable_cc_new)
+			if (!(cc_config_new & CC_DETACH))
 				usleep(PD_T_ERROR_RECOVERY);
 		}
 
-		/* Accept new cc/dts/src value */
-		disable_cc = disable_cc_new;
-		disable_dts_mode = disable_dts_new;
-		allow_src_mode = allow_src_new;
+		/* Accept new cc_config value */
+		cc_config = cc_config_new;
 
-		if (!disable_cc) {
+		if (!(cc_config & CC_DETACH)) {
 			/* Can we source? */
-			dualrole = allow_src_mode && charge_port_is_active();
-			pd_set_dual_role(DUT, dualrole ?
-				PD_DRP_FORCE_SOURCE : PD_DRP_FORCE_SINK);
-
+			chargeable = is_charge_through_allowed();
+			dualrole = chargeable ? get_dual_role_of_src() :
+						PD_DRP_FORCE_SINK;
+			pd_set_dual_role(DUT, dualrole);
 			/*
-			 * Present Rp or Rd on CC1 and CC2 based on
-			 * whether we can source or not.
+			 * If force_source or force_sink role, explicitly set
+			 * the Rp or Rd resistors on CC lines.
+			 *
+			 * If DRP role, don't set any CC pull resistor, the PD
+			 * state machine will toggle and set the pull resistors
+			 * when needed.
 			 */
-			pd_set_host_mode(DUT, dualrole);
-			pd_comm_enable(DUT, dualrole);
+			if (dualrole != PD_DRP_TOGGLE_ON)
+				pd_set_host_mode(DUT, chargeable);
+			pd_comm_enable(DUT, chargeable);
 		}
 	}
 }
 
 static int command_cc(int argc, char **argv)
 {
-	int disable_cc_new;
-	int disable_dts_new;
-	int allow_src_new;
+	int cc_config_new = cc_config;
 
 	if (argc < 2) {
 		print_cc_mode();
@@ -778,49 +809,47 @@ static int command_cc(int argc, char **argv)
 	}
 
 	if (!strcasecmp(argv[1], "off")) {
-		disable_cc_new = 1;
-		disable_dts_new = 0;
-		allow_src_new = 0;
-	} else if (!strcasecmp(argv[1], "src")) {
-		disable_cc_new = 0;
-		disable_dts_new = 1;
-		allow_src_new = 1;
-	} else if (!strcasecmp(argv[1], "snk")) {
-		disable_cc_new = 0;
-		disable_dts_new = 1;
-		allow_src_new = 0;
-	} else if (!strcasecmp(argv[1], "srcdts")) {
-		disable_cc_new = 0;
-		disable_dts_new = 0;
-		allow_src_new = 1;
-	} else if (!strcasecmp(argv[1], "snkdts")) {
-		disable_cc_new = 0;
-		disable_dts_new = 0;
-		allow_src_new = 0;
+		cc_config_new |= CC_DETACH;
+	} else if (!strcasecmp(argv[1], "on")) {
+		cc_config_new &= ~CC_DETACH;
 	} else {
-		ccprintf("Try one of off, src, snk, srcdts, snkdts\n");
-		return EC_ERROR_PARAM2;
+		cc_config_new &= ~CC_DETACH;
+		if (!strcasecmp(argv[1], "src"))
+			cc_config_new = CONFIG_SRC(cc_config_new);
+		else if (!strcasecmp(argv[1], "snk"))
+			cc_config_new = CONFIG_SNK(cc_config_new);
+		else if (!strcasecmp(argv[1], "drp"))
+			cc_config_new = CONFIG_DRP(cc_config_new);
+		else if (!strcasecmp(argv[1], "srcdts"))
+			cc_config_new = CONFIG_SRCDTS(cc_config_new);
+		else if (!strcasecmp(argv[1], "snkdts"))
+			cc_config_new = CONFIG_SNKDTS(cc_config_new);
+		else if (!strcasecmp(argv[1], "drpdts"))
+			cc_config_new = CONFIG_DRPDTS(cc_config_new);
+		else
+			return EC_ERROR_PARAM2;
 	}
-	do_cc(disable_cc_new, disable_dts_new, allow_src_new);
+
+	do_cc(cc_config_new);
 	print_cc_mode();
 
 	return EC_SUCCESS;
 }
 DECLARE_CONSOLE_COMMAND(cc, command_cc,
-			"off|src|snk|srcdts|snkdts",
+			"[off|on|src|snk|drp|srcdts|snkdts|drpdts]",
 			"Servo_v4 DTS and CHG mode");
 
 static void fake_disconnect_end(void)
 {
 	/* Reenable CC lines with previous dts and src modes */
-	do_cc(0, disable_dts_mode, allow_src_mode);
+	do_cc(cc_config & ~CC_DETACH);
 }
 DECLARE_DEFERRED(fake_disconnect_end);
 
 static void fake_disconnect_start(void)
 {
 	/* Disable CC lines */
-	do_cc(1, disable_dts_mode, allow_src_mode);
+	do_cc(cc_config | CC_DETACH);
 
 	hook_call_deferred(&fake_disconnect_end_data,
 			   fake_pd_disconnect_duration_us);
@@ -856,3 +885,24 @@ static int cmd_fake_disconnect(int argc, char *argv[])
 }
 DECLARE_CONSOLE_COMMAND(fakedisconnect, cmd_fake_disconnect,
 			"<delay_ms> <duration_ms>", NULL);
+
+static int cmd_usbc_action(int argc, char *argv[])
+{
+	if (argc != 2)
+		return EC_ERROR_PARAM_COUNT;
+
+	if (!strcasecmp(argv[1], "drp")) {
+		/* Toggle the DRP state, compatible with Plankton. */
+		do_cc(cc_config ^ CC_ENABLE_DRP);
+		CPRINTF("DRP = %d, host_mode = %d\n",
+			!!(cc_config & CC_ENABLE_DRP),
+			!!(cc_config & CC_ALLOW_SRC));
+	} else {
+		return EC_ERROR_PARAM1;
+	}
+
+	return EC_SUCCESS;
+}
+DECLARE_CONSOLE_COMMAND(usbc_action, cmd_usbc_action,
+			"<drp>",
+			"Set Servo v4 type-C port state");