1 files changed, 149 insertions, 57 deletions

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;
 }