servo_v4: add per port dualrole setting

This adds support to configure dualrole setting
per port, so that servo v4 can adjust charge and
dut port separately.

servo will detect charge capability on CHG port
and choose source or sink as appropriate.

Fix null dereference bug in genvif duel to dynamic src_pdo.

"cc" command allows src, snk, srcdts, snkdts configurations.

BRANCH=None
BUG=b:72557427
TEST=charge through and also passive hub. Note Dru doesn't accept DTS hub.
TEST=make buildall -j

Change-Id: I19f1d1a5c37647fec72202191faa4821c06fb460
Signed-off-by: Nick Sanders <nsanders@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1096654
Reviewed-by: Aseda Aboagye <aaboagye@chromium.org>

2 files changed, 150 insertions, 62 deletions

diff --git a/board/servo_v4/board.h b/board/servo_v4/board.h
index 271daa2782..1493d24fc8 100644
--- a/board/servo_v4/board.h
+++ b/board/servo_v4/board.h
@@ -22,7 +22,6 @@
 
 /* Enable USART1,3,4 and USB streams */
 #define CONFIG_STREAM_USART
-
 #define CONFIG_STREAM_USART3
 #define CONFIG_STREAM_USART4
 #define CONFIG_STREAM_USB
@@ -102,12 +101,9 @@
 #define CONFIG_USB_PD_PULLUP TYPEC_RP_USB
 #define CONFIG_USB_PD_VBUS_MEASURE_NOT_PRESENT
 
-/* Override PD_ROLE_DEFAULT in usb_pd.h */
-#define PD_ROLE_DEFAULT(port) ((port) ? PD_ROLE_SOURCE : PD_ROLE_SINK)
-
 /* Don't automatically change roles */
 #undef CONFIG_USB_PD_INITIAL_DRP_STATE
-#define CONFIG_USB_PD_INITIAL_DRP_STATE PD_DRP_FREEZE
+#define CONFIG_USB_PD_INITIAL_DRP_STATE PD_DRP_FORCE_SINK
 
 /* Variable-current Rp no connect and Ra attach macros */
 #define CC_NC(port, cc, sel)  (pd_tcpc_cc_nc(port, cc, sel))
diff --git a/board/servo_v4/usb_pd_policy.c b/board/servo_v4/usb_pd_policy.c
index 0608060ad9..06462405f8 100644
--- a/board/servo_v4/usb_pd_policy.c
+++ b/board/servo_v4/usb_pd_policy.c
@@ -44,9 +44,7 @@ static const uint16_t pd_src_voltages_mv[] = {
 };
 static uint32_t pd_src_chg_pdo[ARRAY_SIZE(pd_src_voltages_mv)];
 static uint8_t chg_pdo_cnt;
-static const uint32_t pd_src_host_pdo[] = {
-		PDO_FIXED(5000, 500, DUT_PDO_FIXED_FLAGS),
-};
+
 const uint32_t pd_snk_pdo[] = {
 		PDO_FIXED(5000, 500, CHG_PDO_FIXED_FLAGS),
 		PDO_BATT(4750, 21000, 15000),
@@ -62,7 +60,14 @@ static struct vbus_prop vbus[CONFIG_USB_PD_PORT_COUNT];
 static int active_charge_port = CHARGE_PORT_NONE;
 static enum charge_supplier active_charge_supplier;
 static uint8_t vbus_rp = TYPEC_RP_RESERVED;
+
+/*
+ * 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;
 
 /* Voltage thresholds for no connect in DTS mode */
 static int pd_src_vnc_dts[TYPEC_RP_RESERVED][2] = {
@@ -107,9 +112,26 @@ static int charge_port_is_active(void)
 	return active_charge_port == CHG && vbus[CHG].mv > 0;
 }
 
+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) {
+		CPRINTS("Enable DUT charge through");
+		pd_set_dual_role(DUT, PD_DRP_FORCE_SOURCE);
+		pd_config_init(DUT, PD_ROLE_SOURCE);
+		pd_update_contract(DUT);
+	}
+}
+DECLARE_DEFERRED(dut_allow_charge);
+
 static void board_manage_dut_port(void)
 {
-	int rp;
+	enum pd_dual_role_states allowed_role;
+	enum pd_dual_role_states current_role;
 
 	/*
 	 * This function is called by the CHG port whenever there has been a
@@ -118,21 +140,28 @@ static void board_manage_dut_port(void)
 	 * contract if it is connected.
 	 */
 
-	/* Assume the default value of Rp */
-	rp = TYPEC_RP_USB;
-	if (vbus[CHG].mv == PD_MIN_MV && charge_port_is_active()) {
-		/* Only advertise higher current via Rp if vbus == 5V */
-		if (vbus[CHG].ma >= 3000)
-			/* CHG port is connected and DUt can advertise 3A */
-			rp = TYPEC_RP_3A0;
-		else if (vbus[CHG].ma >= 1500)
-			rp = TYPEC_RP_1A5;
-	}
+	/* Assume the default value of Rd */
+	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;
 
-	/* Check if Rp setting needs to change from current value */
-	if (vbus_rp != rp)
-		/* Present new Rp value */
-		tcpm_select_rp_value(DUT, rp);
+	current_role = pd_get_dual_role(DUT);
+	if (current_role != allowed_role) {
+		/* Update role. */
+		if (allowed_role == PD_DRP_FORCE_SINK) {
+			/* We've lost charge through. Disable VBUS. */
+			gpio_set_level(GPIO_DUT_CHG_EN, 0);
+
+			/* Mark as SNK only. */
+			pd_set_dual_role(DUT, PD_DRP_FORCE_SINK);
+			pd_config_init(DUT, PD_ROLE_SINK);
+		} else {
+			/* Allow charge through after PD negotiate. */
+			hook_call_deferred(&dut_allow_charge_data, 2000 * MSEC);
+		}
+	}
 
 	/*
 	 * Update PD contract to reflect new available CHG
@@ -151,7 +180,7 @@ static void update_ports(void)
 	 * state
 	 */
 	if (!charge_port_is_active()) {
-		/* CHG Vbus has dropped, so always source DUT Vbus from host */
+		/* CHG Vbus has dropped, so become SNK. */
 		chg_pdo_cnt = 0;
 	} else {
 		/* Advertise the 'best' PDOs at various discrete voltages */
@@ -196,12 +225,6 @@ static void update_ports(void)
 
 	/* Call DUT port manager to update Rp and possible PD contract */
 	board_manage_dut_port();
-
-	/*
-	 * Supply VBUS from the CHG port if available. This may glitch VBUS
-	 * on the DUT during switchover.
-	 */
-	gpio_set_level(GPIO_HOST_OR_CHG_CTL, chg_pdo_cnt > 0);
 }
 
 int board_set_active_charge_port(int charge_port)
@@ -399,18 +422,15 @@ int board_select_rp_value(int port, int rp)
 
 int charge_manager_get_source_pdo(const uint32_t **src_pdo, const int port)
 {
-	int pdo_cnt;
+	int pdo_cnt = 0;
 
 	/*
 	 * If CHG is providing VBUS, then advertise what's available on the CHG
-	 * port, otherwise used the fixed value that matches host capabilities.
+	 * port, otherwise we provide no power.
 	 */
 	if (charge_port_is_active()) {
 		*src_pdo =  pd_src_chg_pdo;
 		pdo_cnt = chg_pdo_cnt;
-	} else {
-		*src_pdo =  pd_src_host_pdo;
-		pdo_cnt = ARRAY_SIZE(pd_src_host_pdo);
 	}
 
 	return pdo_cnt;
@@ -463,20 +483,23 @@ int pd_set_power_supply_ready(int port)
 	if (port == CHG)
 		return EC_ERROR_INVAL;
 
-	/* Enable VBUS */
-	gpio_set_level(GPIO_DUT_CHG_EN, 1);
-
 	if (charge_port_is_active()) {
+		/* Enable VBUS */
+		gpio_set_level(GPIO_DUT_CHG_EN, 1);
+
 		if (vbus[CHG].mv != PD_MIN_MV)
 			CPRINTS("ERROR, CHG port voltage %d != PD_MIN_MV",
 				vbus[CHG].mv);
 
 		vbus[DUT].mv = vbus[CHG].mv;
 		vbus[DUT].ma = vbus[CHG].mv;
+		pd_set_dual_role(DUT, PD_DRP_FORCE_SOURCE);
 	} else {
-		/* Host vbus is always 5V/500mA */
-		vbus[DUT].mv = PD_MIN_MV;
-		vbus[DUT].ma = 500;
+		vbus[DUT].mv = 0;
+		vbus[DUT].ma = 0;
+		gpio_set_level(GPIO_DUT_CHG_EN, 0);
+		pd_set_dual_role(DUT, PD_DRP_FORCE_SINK);
+		return EC_ERROR_NOT_POWERED;
 	}
 
 	/* Enable CCD, if debuggable TS attached */
@@ -497,10 +520,6 @@ void pd_power_supply_reset(int port)
 	/* Disable VBUS */
 	gpio_set_level(GPIO_DUT_CHG_EN, 0);
 
-	/* Host vbus is always 5V/500mA */
-	vbus[DUT].mv = 0;
-	vbus[DUT].ma = 0;
-
 	/* DUT is lost, back to 5V limit on CHG */
 	pd_set_external_voltage_limit(CHG, PD_MIN_MV);
 }
@@ -526,6 +545,14 @@ int pd_check_power_swap(int port)
 	 * SRC. Let servo_v4 have more control over its power role by always
 	 * rejecting power swap requests from the DUT.
 	 */
+
+	/* Port 0 can never provide vbus. */
+	if (port == CHG)
+		return 0;
+
+	if (pd_snk_is_vbus_provided(CHG))
+		return 1;
+
 	return 0;
 }
 
@@ -597,14 +624,91 @@ int pd_custom_vdm(int port, int cnt, uint32_t *payload,
 const struct svdm_amode_fx supported_modes[] = {};
 const int supported_modes_cnt = ARRAY_SIZE(supported_modes);
 
+
+static void print_cc_mode(void)
+{
+	/* Get current CCD status */
+	ccprintf("dts mode: %s\n", disable_dts_mode ? "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");
+}
+
+
+static void do_cc(int disable_dts_new, int allow_src_new)
+{
+	if ((disable_dts_new != disable_dts_mode) ||
+	    (allow_src_new != allow_src_mode)) {
+		/* 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;
+		/* Remove Rp/Rd on both CC lines */
+		board_select_rp_value(DUT, TYPEC_RP_RESERVED);
+
+		/* Some time for DUT to detach, use tErrorRecovery */
+		msleep(25);
+
+		/* Accept new dts/src value */
+		disable_dts_mode = disable_dts_new;
+		allow_src_mode = allow_src_new;
+		/* Can we charge? */
+		pd_set_dual_role(DUT,
+			allow_src_mode && charge_port_is_active() ?
+			PD_DRP_FORCE_SOURCE : PD_DRP_FORCE_SINK);
+
+		/* Present Rp or Rd on CC1 and CC2 based on disable_dts_mode */
+		pd_config_init(DUT,
+			pd_get_dual_role(DUT) == PD_DRP_FORCE_SOURCE);
+	}
+
+	print_cc_mode();
+}
+
+static int command_cc(int argc, char **argv)
+{
+	int disable_dts_new;
+	int allow_src_new;
+
+	if (argc < 2) {
+		print_cc_mode();
+		return EC_SUCCESS;
+	}
+
+	if (!strcasecmp(argv[1], "src")) {
+		disable_dts_new = 1;
+		allow_src_new = 1;
+	} else if (!strcasecmp(argv[1], "snk")) {
+		disable_dts_new = 1;
+		allow_src_new = 0;
+	} else if (!strcasecmp(argv[1], "srcdts")) {
+		disable_dts_new = 0;
+		allow_src_new = 1;
+	} else if (!strcasecmp(argv[1], "snkdts")) {
+		disable_dts_new = 0;
+		allow_src_new = 0;
+	} else {
+		ccprintf("Try one of src, snk, srcdts, snkdts\n");
+		return EC_ERROR_PARAM2;
+	}
+	do_cc(disable_dts_new, allow_src_new);
+
+	return EC_SUCCESS;
+}
+DECLARE_CONSOLE_COMMAND(cc, command_cc,
+			"src|snk|srcdts|snkdts",
+			"Servo_v4 DTS and CHG mode");
+
+
 static int command_dts(int argc, char **argv)
 {
 	int disable_dts_new;
 	int val;
 
 	if (argc < 2) {
-		/* Get current CCD status */
-		ccprintf("dts mode: %s\n", disable_dts_mode ? "off" : "on");
+		print_cc_mode();
 		return EC_SUCCESS;
 	}
 
@@ -612,21 +716,9 @@ static int command_dts(int argc, char **argv)
 		return EC_ERROR_PARAM2;
 
 	disable_dts_new = val ^ 1;
-	if (disable_dts_new != disable_dts_mode) {
-		/* Force detach */
-		pd_power_supply_reset(DUT);
-		/* Always set to 0 here so both CC lines are changed */
-		disable_dts_mode = 0;
-		/* Remove Rp/Rd on both CC lines */
-		board_select_rp_value(DUT, TYPEC_RP_RESERVED);
-		/* Accept new disable_dts value */
-		disable_dts_mode = disable_dts_new;
-		/* Some time for DUT to detach */
-		msleep(100);
-		/* Present RP_USB on CC1 and CC2 based on disable_dts_mode */
-		board_select_rp_value(DUT, TYPEC_RP_USB);
-		ccprintf("dts mode: %s\n", disable_dts_mode ? "off" : "on");
-	}
+
+	/* Change dts without changing src. */
+	do_cc(disable_dts_new, allow_src_mode);
 
 	return EC_SUCCESS;
 }