servo_v4p1: Add USB PD functionality

This functionality is only available in RO

BRANCH=none
BUG=b:146793000
TEST=make -j buildall

Signed-off-by: Sam Hurst <shurst@google.com>
Change-Id: I3a81284a05c013169d1b401cd2b2056724a06465
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2216408
Reviewed-by: Wai-Hong Tam <waihong@google.com>

6 files changed, 1667 insertions, 3 deletions

diff --git a/board/servo_v4p1/board.c b/board/servo_v4p1/board.c
index 0ccf596ddd..d573308712 100644
--- a/board/servo_v4p1/board.c
+++ b/board/servo_v4p1/board.c
@@ -47,10 +47,12 @@
 #ifdef SECTION_IS_RO
 static void vbus0_evt(enum gpio_signal signal)
 {
+	task_wake(TASK_ID_PD_C0);
 }
 
 static void vbus1_evt(enum gpio_signal signal)
 {
+	task_wake(TASK_ID_PD_C1);
 }
 
 static void tca_evt(enum gpio_signal signal)
@@ -120,6 +122,18 @@ void pd_task(void *u)
 {
 	/* DO NOTHING */
 }
+__override uint8_t board_get_usb_pd_port_count(void)
+{
+	return CONFIG_USB_PD_PORT_MAX_COUNT;
+}
+
+void pd_set_suspend(int port, int suspend)
+{
+	/*
+	 * Do nothing. This is only here to make the linker happy for this
+	 * old board on ToT.
+	 */
+}
 #endif /* SECTION_IS_RO */
 
 #include "gpio_list.h"
@@ -283,6 +297,7 @@ const unsigned int i2c_ports_used = ARRAY_SIZE(i2c_ports);
 
 int usb_i2c_board_is_enabled(void) { return 1; }
 
+
 /******************************************************************************
  * Initialize board.
  */
@@ -338,6 +353,12 @@ static void board_init(void)
 	/* Disable power to DUT by default */
 	chg_power_select(CHG_POWER_OFF);
 
+	/*
+	 * Voltage transition needs to occur in lockstep between the CHG and
+	 * DUT ports, so initially limit voltage to 5V.
+	 */
+	pd_set_max_voltage(PD_MIN_MV);
+
 	/* Start SuzyQ detection */
 	start_ccd_meas_sbu_cycle();
 #endif /* SECTION_IS_RO */
diff --git a/board/servo_v4p1/board.h b/board/servo_v4p1/board.h
index a71e92c5ab..471dd93177 100644
--- a/board/servo_v4p1/board.h
+++ b/board/servo_v4p1/board.h
@@ -19,11 +19,9 @@
 /* Servo V4.1 Ports:
  *  CHG - port 0
  *  DUT - port 1
- *  ALT - port 2
  */
 #define CHG 0
 #define DUT 1
-#define ALT 2
 
 /*
  * Flash layout: we redefine the sections offsets and sizes as we want to
@@ -176,6 +174,30 @@
 
 #ifdef SECTION_IS_RO
 #define CONFIG_INA231
+#define CONFIG_CHARGE_MANAGER
+#undef  CONFIG_CHARGE_MANAGER_SAFE_MODE
+#define CONFIG_USB_POWER_DELIVERY
+#define CONFIG_USB_PD_TCPMV1
+#define CONFIG_CMD_PD
+#define CONFIG_USB_PD_CUSTOM_PDO
+#define CONFIG_USB_PD_DUAL_ROLE
+#define CONFIG_USB_PD_DYNAMIC_SRC_CAP
+#define CONFIG_USB_PD_INTERNAL_COMP
+#define CONFIG_USB_PD_TCPC
+#define CONFIG_USB_PD_TCPM_STUB
+#undef CONFIG_USB_PD_PULLUP
+#define CONFIG_USB_PD_PULLUP TYPEC_RP_USB
+#define CONFIG_USB_PD_VBUS_MEASURE_NOT_PRESENT
+#define CONFIG_USB_PD_ALT_MODE
+
+/* Don't automatically change roles */
+#undef CONFIG_USB_PD_INITIAL_DRP_STATE
+#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))
+#define CC_RA(port, cc, sel)  (pd_tcpc_cc_ra(port, cc, sel))
+
 /*
  * TODO(crosbug.com/p/60792): The delay values are currently just place holders
  * and the delay will need to be relative to the circuitry that allows VBUS to
@@ -292,6 +314,13 @@ int pd_set_rp_rd(int port, int cc_pull, int rp_value);
 int board_get_version(void);
 
 /**
+ * Enable or disable external HPD detection
+ *
+ * @param enable Enable external HPD detection if true, otherwise disable
+ */
+void ext_hpd_detection_enable(int enable);
+
+/**
  * Enable or disable CCD
  *
  * @param enable Enable CCD if true, otherwise disable
diff --git a/board/servo_v4p1/build.mk b/board/servo_v4p1/build.mk
index 09711e0beb..7c4e8b030f 100644
--- a/board/servo_v4p1/build.mk
+++ b/board/servo_v4p1/build.mk
@@ -23,5 +23,6 @@ board-ro+=ccd_measure_sbu.o
 board-ro+=pathsel.o
 board-ro+=chg_control.o
 board-ro+=ina231s.o
+board-ro+=usb_pd_policy.o
 
 all_deps=$(patsubst ro,,$(def_all_deps))
diff --git a/board/servo_v4p1/ec.tasklist b/board/servo_v4p1/ec.tasklist
index 411353df5d..700315ec90 100644
--- a/board/servo_v4p1/ec.tasklist
+++ b/board/servo_v4p1/ec.tasklist
@@ -8,4 +8,6 @@
  */
 #define CONFIG_TASK_LIST \
 	TASK_ALWAYS(HOOKS,   hook_task,    NULL, VENTI_TASK_STACK_SIZE) \
-	TASK_ALWAYS(CONSOLE, console_task, NULL, VENTI_TASK_STACK_SIZE)
+	TASK_ALWAYS(CONSOLE, console_task, NULL, VENTI_TASK_STACK_SIZE) \
+	TASK_ALWAYS(PD_C0,   pd_task,      NULL, VENTI_TASK_STACK_SIZE) \
+	TASK_ALWAYS(PD_C1,   pd_task,      NULL, VENTI_TASK_STACK_SIZE)
diff --git a/board/servo_v4p1/usb_pd_config.h b/board/servo_v4p1/usb_pd_config.h
new file mode 100644
index 0000000000..8bf1e6335b
--- /dev/null
+++ b/board/servo_v4p1/usb_pd_config.h
@@ -0,0 +1,293 @@
+/* Copyright 2020 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "adc.h"
+#include "chip/stm32/registers.h"
+#include "console.h"
+#include "gpio.h"
+#include "ec_commands.h"
+#include "usb_pd_tcpm.h"
+
+/* USB Power delivery board configuration */
+
+#ifndef __CROS_EC_USB_PD_CONFIG_H
+#define __CROS_EC_USB_PD_CONFIG_H
+
+/* NOTES: Servo V4 and glados equivalents:
+ *	Glados		Servo V4
+ *	C0		CHG
+ *	C1		DUT
+ *
+ */
+#define CHG 0
+#define DUT 1
+
+/* Timer selection for baseband PD communication */
+#define TIM_CLOCK_PD_TX_CHG 16
+#define TIM_CLOCK_PD_RX_CHG 1
+#define TIM_CLOCK_PD_TX_DUT 15
+#define TIM_CLOCK_PD_RX_DUT 3
+
+/* Timer channel */
+#define TIM_TX_CCR_CHG 1
+#define TIM_RX_CCR_CHG 1
+#define TIM_TX_CCR_DUT 2
+#define TIM_RX_CCR_DUT 1
+
+#define TIM_CLOCK_PD_TX(p) ((p) ? TIM_CLOCK_PD_TX_DUT : TIM_CLOCK_PD_TX_CHG)
+#define TIM_CLOCK_PD_RX(p) ((p) ? TIM_CLOCK_PD_RX_DUT : TIM_CLOCK_PD_RX_CHG)
+
+/* RX timer capture/compare register */
+#define TIM_CCR_CHG (&STM32_TIM_CCRx(TIM_CLOCK_PD_RX_CHG, TIM_RX_CCR_CHG))
+#define TIM_CCR_DUT (&STM32_TIM_CCRx(TIM_CLOCK_PD_RX_DUT, TIM_RX_CCR_DUT))
+#define TIM_RX_CCR_REG(p) ((p) ? TIM_CCR_DUT : TIM_CCR_CHG)
+
+/* TX and RX timer register */
+#define TIM_REG_TX_CHG (STM32_TIM_BASE(TIM_CLOCK_PD_TX_CHG))
+#define TIM_REG_RX_CHG (STM32_TIM_BASE(TIM_CLOCK_PD_RX_CHG))
+#define TIM_REG_TX_DUT (STM32_TIM_BASE(TIM_CLOCK_PD_TX_DUT))
+#define TIM_REG_RX_DUT (STM32_TIM_BASE(TIM_CLOCK_PD_RX_DUT))
+#define TIM_REG_TX(p) ((p) ? TIM_REG_TX_DUT : TIM_REG_TX_CHG)
+#define TIM_REG_RX(p) ((p) ? TIM_REG_RX_DUT : TIM_REG_RX_CHG)
+
+/* use the hardware accelerator for CRC */
+#define CONFIG_HW_CRC
+
+/* Servo v4 CC configuration */
+#define CC_DETACH	BIT(0)   /* Emulate detach: both CC open */
+#define CC_DISABLE_DTS	BIT(1)   /* Apply resistors to single or both CC? */
+#define CC_ALLOW_SRC	BIT(2)   /* Allow charge through by policy? */
+#define CC_ENABLE_DRP	BIT(3)   /* Enable dual-role port */
+#define CC_SNK_WITH_PD	BIT(4)   /* Force enabling PD comm for sink role */
+#define CC_POLARITY	BIT(5)   /* CC polarity */
+#define CC_EMCA_SERVO	BIT(6)   /*
+				  * Emulate Electronically Marked Cable Assembly
+				  * (EMCA) servo (or non-EMCA)
+				  */
+
+/* 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)
+{
+	if (port == 0)
+		STM32_RCC_APB2ENR |= STM32_RCC_PB2_SPI1;
+	else
+		STM32_RCC_APB1ENR |= STM32_RCC_PB1_SPI2;
+}
+
+/* DMA for transmit uses DMA CH3 for CHG and DMA_CH7 for DUT */
+#define DMAC_SPI_TX(p) ((p) ? STM32_DMAC_CH7 : STM32_DMAC_CH3)
+
+/* RX uses COMP1 and TIM1_CH1 on port CHG and COMP2 and TIM3_CH1 for port DUT*/
+/* DUT RX use CMP1, TIM3_CH1, DMA_CH6 */
+#define CMP1OUTSEL STM32_COMP_CMP1OUTSEL_TIM3_IC1
+/* CHG RX use CMP2, TIM1_CH1, DMA_CH2 */
+#define CMP2OUTSEL STM32_COMP_CMP2OUTSEL_TIM1_IC1
+
+#define TIM_TX_CCR_IDX(p) ((p) ? TIM_TX_CCR_DUT : TIM_TX_CCR_CHG)
+#define TIM_RX_CCR_IDX(p) ((p) ? TIM_RX_CCR_DUT : TIM_RX_CCR_CHG)
+#define TIM_CCR_CS  1
+
+/*
+ * EXTI line 21 is connected to the CMP1 output,
+ * EXTI line 22 is connected to the CMP2 output,
+ * CHG uses CMP2, and DUT uses CMP1.
+ */
+#define EXTI_COMP_MASK(p) ((p) ? (1<<21) : BIT(22))
+
+#define IRQ_COMP STM32_IRQ_COMP
+/* triggers packet detection on comparator falling edge */
+#define EXTI_XTSR STM32_EXTI_FTSR
+
+/* DMA for receive uses DMA_CH2 for CHG and DMA_CH6 for DUT */
+#define DMAC_TIM_RX(p) ((p) ? STM32_DMAC_CH6 : STM32_DMAC_CH2)
+
+/* the pins used for communication need to be hi-speed */
+static inline void pd_set_pins_speed(int port)
+{
+	if (port == 0) {
+		/* 40 MHz pin speed on SPI PB3&4,
+		 * (USB_CHG_TX_CLKIN & USB_CHG_CC1_TX_DATA)
+		 */
+		STM32_GPIO_OSPEEDR(GPIO_B) |= 0x000003C0;
+		/* 40 MHz pin speed on TIM16_CH1 (PB8),
+		 * (USB_CHG_TX_CLKOUT)
+		 */
+		STM32_GPIO_OSPEEDR(GPIO_B) |= 0x00030000;
+	} else {
+		/* 40 MHz pin speed on SPI PB13/14,
+		 * (USB_DUT_TX_CLKIN & USB_DUT_CC1_TX_DATA)
+		 */
+		STM32_GPIO_OSPEEDR(GPIO_B) |= 0x3C000000;
+		/* 40 MHz pin speed on TIM15_CH2 (PB15) */
+		STM32_GPIO_OSPEEDR(GPIO_B) |= 0xC0000000;
+	}
+}
+
+/* Reset SPI peripheral used for TX */
+static inline void pd_tx_spi_reset(int port)
+{
+	if (port == 0) {
+		/* Reset SPI1 */
+		STM32_RCC_APB2RSTR |= BIT(12);
+		STM32_RCC_APB2RSTR &= ~BIT(12);
+	} else {
+		/* Reset SPI2 */
+		STM32_RCC_APB1RSTR |= BIT(14);
+		STM32_RCC_APB1RSTR &= ~BIT(14);
+	}
+}
+
+/* Drive the CC line from the TX block */
+static inline void pd_tx_enable(int port, int polarity)
+{
+	if (port == 0) {
+		/* put SPI function on TX pin */
+		if (polarity) {
+			const struct gpio_info *g = gpio_list +
+				GPIO_USB_CHG_CC2_TX_DATA;
+			gpio_set_alternate_function(g->port, g->mask, 0);
+
+			/* set the low level reference */
+			gpio_set_flags(GPIO_USB_CHG_CC2_MCU, GPIO_OUT_LOW);
+		} else {
+			const struct gpio_info *g = gpio_list +
+				GPIO_USB_CHG_CC1_TX_DATA;
+			gpio_set_alternate_function(g->port, g->mask, 0);
+
+			/* set the low level reference */
+			gpio_set_flags(GPIO_USB_CHG_CC1_MCU, GPIO_OUT_LOW);
+		}
+	} else {
+		/* put SPI function on TX pin */
+		/* MCU ADC pin output low */
+		if (polarity) {
+			/* USB_DUT_CC2_TX_DATA: PC2 is SPI2 MISO */
+			const struct gpio_info *g = gpio_list +
+				GPIO_USB_DUT_CC2_TX_DATA;
+			gpio_set_alternate_function(g->port, g->mask, 1);
+
+			/* set the low level reference */
+			gpio_set_flags(GPIO_USB_DUT_CC2_MCU, GPIO_OUT_LOW);
+		} else {
+			/* USB_DUT_CC1_TX_DATA: PB14 is SPI2 MISO */
+			const struct gpio_info *g = gpio_list +
+				GPIO_USB_DUT_CC1_TX_DATA;
+			gpio_set_alternate_function(g->port, g->mask, 0);
+
+			/* set the low level reference */
+			gpio_set_flags(GPIO_USB_DUT_CC1_MCU, GPIO_OUT_LOW);
+		}
+	}
+}
+
+/* Put the TX driver in Hi-Z state */
+static inline void pd_tx_disable(int port, int polarity)
+{
+	if (port == 0) {
+		if (polarity) {
+			gpio_set_flags(GPIO_USB_CHG_CC2_TX_DATA, GPIO_INPUT);
+			gpio_set_flags(GPIO_USB_CHG_CC2_MCU, GPIO_ANALOG);
+		} else {
+			gpio_set_flags(GPIO_USB_CHG_CC1_TX_DATA, GPIO_INPUT);
+			gpio_set_flags(GPIO_USB_CHG_CC1_MCU, GPIO_ANALOG);
+		}
+	} else {
+		if (polarity) {
+			gpio_set_flags(GPIO_USB_DUT_CC2_TX_DATA, GPIO_INPUT);
+			gpio_set_flags(GPIO_USB_DUT_CC2_MCU, GPIO_ANALOG);
+		} else {
+			gpio_set_flags(GPIO_USB_DUT_CC1_TX_DATA, GPIO_INPUT);
+			gpio_set_flags(GPIO_USB_DUT_CC1_MCU, GPIO_ANALOG);
+		}
+	}
+}
+
+/* we know the plug polarity, do the right configuration */
+static inline void pd_select_polarity(int port, int polarity)
+{
+	uint32_t val = STM32_COMP_CSR;
+
+	/* Use window mode so that COMP1 and COMP2 share non-inverting input */
+	val |= STM32_COMP_CMP1EN | STM32_COMP_CMP2EN | STM32_COMP_WNDWEN;
+
+	if (port == 0) {
+		/* CHG use the right comparator inverted input for COMP2 */
+		STM32_COMP_CSR = (val & ~STM32_COMP_CMP2INSEL_MASK) |
+			(polarity ? STM32_COMP_CMP2INSEL_INM4  /* PA4: C0_CC2 */
+				  : STM32_COMP_CMP2INSEL_INM6);/* PA2: C0_CC1 */
+	} else {
+		/* DUT use the right comparator inverted input for COMP1 */
+		STM32_COMP_CSR = (val & ~STM32_COMP_CMP1INSEL_MASK) |
+			(polarity ? STM32_COMP_CMP1INSEL_INM5  /* PA5: C1_CC2 */
+			 : STM32_COMP_CMP1INSEL_INM6);/* PA0: C1_CC1 */
+	}
+}
+
+/* Initialize pins used for TX and put them in Hi-Z */
+static inline void pd_tx_init(void)
+{
+	gpio_config_module(MODULE_USB_PD, 1);
+}
+
+static inline void pd_set_host_mode(int port, int enable)
+{
+	/*
+	 * CHG (port == 0) port has fixed Rd attached and therefore can only
+	 * present as a SNK device. If port != DUT (port == 1), then nothing to
+	 * do in this function.
+	 */
+	if (!port)
+		return;
+
+	if (enable) {
+		/*
+		 * Servo_v4 in SRC mode acts as a DTS (debug test
+		 * accessory) and needs to present Rp on both CC
+		 * lines. In order to support orientation detection, and
+		 * advertise the correct TypeC current level, the
+		 * values of Rp1/Rp2 need to asymmetric with Rp1 > Rp2. This
+		 * function is called without a specified Rp value so assume the
+		 * servo_v4 default of USB level current. If a higher current
+		 * can be supported, then the Rp value will get adjusted when
+		 * VBUS is enabled.
+		 */
+		pd_set_rp_rd(port, TYPEC_CC_RP, TYPEC_RP_USB);
+
+		gpio_set_flags(GPIO_USB_DUT_CC1_TX_DATA, GPIO_INPUT);
+		gpio_set_flags(GPIO_USB_DUT_CC2_TX_DATA, GPIO_INPUT);
+	} else {
+		/* Select Rd, the Rp value is a don't care */
+		pd_set_rp_rd(port, TYPEC_CC_RD, TYPEC_RP_RESERVED);
+	}
+}
+
+/**
+ * Initialize various GPIOs and interfaces to safe state at start of pd_task.
+ *
+ * These include:
+ *   VBUS, charge path based on power role.
+ *   Physical layer CC transmit.
+ *
+ * @param port        USB-C port number
+ * @param power_role  Power role of device
+ */
+static inline void pd_config_init(int port, uint8_t power_role)
+{
+	/*
+	 * Set CC pull resistors. The PD state machine will then transit and
+	 * enable VBUS after it detects valid voltages on CC lines.
+	 */
+	pd_set_host_mode(port, power_role);
+
+	/* Initialize TX pins and put them in Hi-Z */
+	pd_tx_init();
+
+}
+
+int pd_adc_read(int port, int cc);
+
+#endif /* __CROS_EC_USB_PD_CONFIG_H */
+
diff --git a/board/servo_v4p1/usb_pd_policy.c b/board/servo_v4p1/usb_pd_policy.c
new file mode 100644
index 0000000000..e094557d3d
--- /dev/null
+++ b/board/servo_v4p1/usb_pd_policy.c
@@ -0,0 +1,1318 @@
+/* Copyright 2020 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "atomic.h"
+#include "chg_control.h"
+#include "charge_manager.h"
+#include "common.h"
+#include "console.h"
+#include "gpio.h"
+#include "hooks.h"
+#include "host_command.h"
+#include "i2c.h"
+#include "ioexpanders.h"
+#include "registers.h"
+#include "system.h"
+#include "task.h"
+#include "tcpm.h"
+#include "timer.h"
+#include "util.h"
+#include "usb_common.h"
+#include "usb_mux.h"
+#include "usb_pd.h"
+#include "usb_pd_config.h"
+#include "usb_pd_tcpm.h"
+
+#define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args)
+#define CPRINTS(format, args...) cprints(CC_USBPD, format, ## args)
+
+#define DUT_PDO_FIXED_FLAGS (PDO_FIXED_DUAL_ROLE | PDO_FIXED_DATA_SWAP |\
+			     PDO_FIXED_COMM_CAP)
+
+#define CHG_PDO_FIXED_FLAGS (PDO_FIXED_DATA_SWAP)
+
+#define VBUS_UNCHANGED(curr, pend, new) (curr == new && pend == new)
+
+/* Macros to config the PD role */
+#define CONF_SET_CLEAR(c, set, clear) ((c | (set)) & ~(clear))
+#define CONF_SRC(c) CONF_SET_CLEAR(c, \
+				CC_DISABLE_DTS | CC_ALLOW_SRC, \
+				CC_ENABLE_DRP | CC_SNK_WITH_PD)
+#define CONF_SNK(c) CONF_SET_CLEAR(c, \
+				CC_DISABLE_DTS, \
+				CC_ALLOW_SRC | CC_ENABLE_DRP | CC_SNK_WITH_PD)
+#define CONF_PDSNK(c) CONF_SET_CLEAR(c, \
+				CC_DISABLE_DTS | CC_SNK_WITH_PD, \
+				CC_ALLOW_SRC | CC_ENABLE_DRP)
+#define CONF_DRP(c) CONF_SET_CLEAR(c, \
+				CC_DISABLE_DTS | CC_ALLOW_SRC | CC_ENABLE_DRP, \
+				CC_SNK_WITH_PD)
+#define CONF_SRCDTS(c) CONF_SET_CLEAR(c, \
+				CC_ALLOW_SRC, \
+				CC_ENABLE_DRP | CC_DISABLE_DTS | CC_SNK_WITH_PD)
+#define CONF_SNKDTS(c) CONF_SET_CLEAR(c, \
+				0, \
+				CC_ALLOW_SRC | CC_ENABLE_DRP | \
+				CC_DISABLE_DTS | CC_SNK_WITH_PD)
+#define CONF_PDSNKDTS(c) CONF_SET_CLEAR(c, \
+				CC_SNK_WITH_PD, \
+				CC_ALLOW_SRC | CC_ENABLE_DRP | CC_DISABLE_DTS)
+#define CONF_DRPDTS(c) CONF_SET_CLEAR(c, \
+				CC_ALLOW_SRC | CC_ENABLE_DRP, \
+				CC_DISABLE_DTS | CC_SNK_WITH_PD)
+
+/* Macros to apply Rd/Rp to CC lines */
+#define DUT_ACTIVE_CC_SET(r, flags) \
+	gpio_set_flags(cc_config & CC_POLARITY ? \
+				CONCAT2(GPIO_USB_DUT_CC2_, r) : \
+				CONCAT2(GPIO_USB_DUT_CC1_, r), \
+		       flags)
+#define DUT_INACTIVE_CC_SET(r, flags) \
+	gpio_set_flags(cc_config & CC_POLARITY ? \
+				CONCAT2(GPIO_USB_DUT_CC1_, r) : \
+				CONCAT2(GPIO_USB_DUT_CC2_, r), \
+		       flags)
+#define DUT_BOTH_CC_SET(r, flags) \
+	do { \
+		gpio_set_flags(CONCAT2(GPIO_USB_DUT_CC1_, r), flags); \
+		gpio_set_flags(CONCAT2(GPIO_USB_DUT_CC2_, r), flags); \
+	} while (0)
+
+#define DUT_ACTIVE_CC_PU(r) DUT_ACTIVE_CC_SET(r, GPIO_OUT_HIGH)
+#define DUT_INACTIVE_CC_PU(r) DUT_INACTIVE_CC_SET(r, GPIO_OUT_HIGH)
+#define DUT_ACTIVE_CC_PD(r) DUT_ACTIVE_CC_SET(r, GPIO_OUT_LOW)
+#define DUT_INACTIVE_CC_PD(r) DUT_INACTIVE_CC_SET(r, GPIO_OUT_LOW)
+#define DUT_BOTH_CC_PD(r) DUT_BOTH_CC_SET(r, GPIO_OUT_LOW)
+#define DUT_BOTH_CC_OPEN(r) DUT_BOTH_CC_SET(r, GPIO_INPUT)
+#define DUT_ACTIVE_CC_OPEN(r) DUT_ACTIVE_CC_SET(r, GPIO_INPUT)
+#define DUT_INACTIVE_CC_OPEN(r) DUT_INACTIVE_CC_SET(r, GPIO_INPUT)
+
+#define TUSB1064_I2C_ADDR_FLAG          0x12
+
+#define TUSB1064_REG_GENERAL            0x0a
+#define TUSB1064_MODE_POLARITY_INVERTED 0x4
+#define TUSB1064_MODE_DP_ENABLED        0x2
+#define TUSB1064_MODE_USB_ENABLED       0x1
+
+#define TUSB1064_REG_DP_CTRL            0x13
+
+/*
+ * 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
+ * entry will be fixed 5V, but its current value may change based on the CHG
+ * port vbus info. Subsequent entries are used for when offering vbus greater
+ * than 5V.
+ */
+static const uint16_t pd_src_voltages_mv[] = {
+		5000, 9000, 10000, 12000, 15000, 20000,
+};
+static uint32_t pd_src_chg_pdo[ARRAY_SIZE(pd_src_voltages_mv)];
+static uint8_t chg_pdo_cnt;
+
+const uint32_t pd_snk_pdo[] = {
+		PDO_FIXED(5000, 500, CHG_PDO_FIXED_FLAGS),
+		PDO_BATT(4750, 21000, 15000),
+		PDO_VAR(4750, 21000, 3000),
+};
+const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
+
+struct vbus_prop {
+	int mv;
+	int ma;
+};
+static struct vbus_prop vbus[CONFIG_USB_PD_PORT_MAX_COUNT];
+static int active_charge_port = CHARGE_PORT_NONE;
+static enum charge_supplier active_charge_supplier;
+static uint8_t vbus_rp = TYPEC_RP_RESERVED;
+
+static int cc_config = CC_ALLOW_SRC | CC_EMCA_SERVO;
+
+/* Voltage thresholds for no connect in DTS mode */
+static int pd_src_vnc_dts[TYPEC_RP_RESERVED][2] = {
+	{PD_SRC_3_0_VNC_MV, PD_SRC_1_5_VNC_MV},
+	{PD_SRC_1_5_VNC_MV, PD_SRC_DEF_VNC_MV},
+	{PD_SRC_3_0_VNC_MV, PD_SRC_DEF_VNC_MV},
+};
+/* Voltage thresholds for Ra attach in DTS mode */
+static int pd_src_rd_threshold_dts[TYPEC_RP_RESERVED][2] = {
+	{PD_SRC_3_0_RD_THRESH_MV, PD_SRC_1_5_RD_THRESH_MV},
+	{PD_SRC_1_5_RD_THRESH_MV, PD_SRC_DEF_RD_THRESH_MV},
+	{PD_SRC_3_0_RD_THRESH_MV, PD_SRC_DEF_RD_THRESH_MV},
+};
+/* Voltage thresholds for no connect in normal SRC mode */
+static int pd_src_vnc[TYPEC_RP_RESERVED] = {
+	PD_SRC_DEF_VNC_MV,
+	PD_SRC_1_5_VNC_MV,
+	PD_SRC_3_0_VNC_MV,
+};
+/* Voltage thresholds for Ra attach in normal SRC mode */
+static int pd_src_rd_threshold[TYPEC_RP_RESERVED] = {
+	PD_SRC_DEF_RD_THRESH_MV,
+	PD_SRC_1_5_RD_THRESH_MV,
+	PD_SRC_3_0_RD_THRESH_MV,
+};
+
+/* Saved value for the duration of faking PD disconnect */
+static int fake_pd_disconnect_duration_us;
+
+/* Shadow what would be in TCPC register state. */
+static int rp_value_stored = TYPEC_RP_USB;
+/*
+ * Make sure the below matches CC_EMCA_SERVO
+ * otherwise you'll have a bad time.
+ */
+static int cc_pull_stored = TYPEC_CC_RD;
+
+static int user_limited_max_mv = 20000;
+
+static uint32_t max_supported_voltage(void)
+{
+	return user_limited_max_mv;
+}
+
+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 (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, 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);
+
+		/*
+		 * Enable PD comm. The PD comm may be disabled during
+		 * the power charge-through was detached.
+		 */
+		pd_comm_enable(DUT, 1);
+
+		pd_update_contract(DUT);
+	}
+}
+DECLARE_DEFERRED(dut_allow_charge);
+
+static void board_manage_dut_port(void)
+{
+	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
+	 * change in its vbus voltage or current. That change may necessitate
+	 * that the DUT port present a different Rp value or renogiate its PD
+	 * contract if it is connected.
+	 */
+
+	/* Assume the default value of Rd */
+	allowed_role = PD_DRP_FORCE_SINK;
+
+	/* If VBUS charge through is available, mark as such. */
+	if (is_charge_through_allowed())
+		allowed_role = get_dual_role_of_src();
+
+	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. */
+			chg_power_select(CHG_POWER_OFF);
+			dut_chg_en(0);
+
+			/* Mark as SNK only. */
+			pd_set_dual_role(DUT, PD_DRP_FORCE_SINK);
+			pd_set_host_mode(DUT, 0);
+
+			/*
+			 * Disable PD comm. It matches the user expectation that
+			 * unplugging the power charge-through makes servo v4 as
+			 * a passive hub, without any PD support.
+			 *
+			 * There is an exception that servo v4 is explicitly set
+			 * to have PD, like the "pnsnk" mode.
+			 */
+			pd_comm_enable(DUT, cc_config & CC_SNK_WITH_PD ? 1 : 0);
+		} else {
+			/* Allow charge through after PD negotiate. */
+			hook_call_deferred(&dut_allow_charge_data, 2000 * MSEC);
+		}
+	}
+
+	/*
+	 * Update PD contract to reflect new available CHG
+	 * voltage/current values.
+	 */
+	pd_update_contract(DUT);
+}
+
+static void update_ports(void)
+{
+	int pdo_index, src_index, snk_index, i;
+	uint32_t pdo, max_ma, max_mv;
+
+	/*
+	 * CHG Vbus has changed states, update PDO that reflects CHG port
+	 * state
+	 */
+	if (!charge_port_is_active()) {
+		/* CHG Vbus has dropped, so become SNK. */
+		chg_pdo_cnt = 0;
+	} else {
+		/* Advertise the 'best' PDOs at various discrete voltages */
+		if (active_charge_supplier == CHARGE_SUPPLIER_PD) {
+			src_index = 0;
+			snk_index = -1;
+
+			for (i = 0; i < ARRAY_SIZE(pd_src_voltages_mv); ++i) {
+				/* Adhere to board voltage limits */
+				if (pd_src_voltages_mv[i] >
+				    max_supported_voltage())
+					break;
+
+				/* Find the 'best' PDO <= voltage */
+				pdo_index =
+				pd_find_pdo_index(pd_get_src_cap_cnt(CHG),
+					pd_get_src_caps(CHG),
+					pd_src_voltages_mv[i], &pdo);
+				/* Don't duplicate PDOs */
+				if (pdo_index == snk_index)
+					continue;
+				/* Skip battery / variable PDOs */
+				if ((pdo & PDO_TYPE_MASK) != PDO_TYPE_FIXED)
+					continue;
+
+				snk_index = pdo_index;
+				pd_extract_pdo_power(pdo, &max_ma, &max_mv);
+				pd_src_chg_pdo[src_index++] =
+					PDO_FIXED_VOLT(max_mv) |
+					PDO_FIXED_CURR(max_ma) |
+					DUT_PDO_FIXED_FLAGS |
+					PDO_FIXED_UNCONSTRAINED;
+			}
+			chg_pdo_cnt = src_index;
+		} else {
+			/* 5V PDO */
+			pd_src_chg_pdo[0] = PDO_FIXED_VOLT(PD_MIN_MV) |
+				PDO_FIXED_CURR(vbus[CHG].ma) |
+				DUT_PDO_FIXED_FLAGS |
+				PDO_FIXED_UNCONSTRAINED;
+
+			chg_pdo_cnt = 1;
+		}
+	}
+
+	/* Call DUT port manager to update Rp and possible PD contract */
+	board_manage_dut_port();
+}
+
+int board_set_active_charge_port(int charge_port)
+{
+	if (charge_port == DUT)
+		return -1;
+
+	active_charge_port = charge_port;
+	update_ports();
+
+	if (!charge_port_is_active())
+		/* Don't negotiate > 5V, except in lockstep with DUT */
+		pd_set_external_voltage_limit(CHG, PD_MIN_MV);
+
+	return 0;
+}
+
+void board_set_charge_limit(int port, int supplier, int charge_ma,
+			    int max_ma, int charge_mv)
+{
+	if (port != CHG)
+		return;
+
+	active_charge_supplier = supplier;
+
+	/* Update the voltage/current values for CHG port */
+	vbus[CHG].ma = charge_ma;
+	vbus[CHG].mv = charge_mv;
+	update_ports();
+}
+
+__override uint8_t board_get_src_dts_polarity(int port)
+{
+	/*
+	 * When servo configured as srcdts, the CC polarity is based
+	 * on the flags.
+	 */
+	if (port == DUT)
+		return !!(cc_config & CC_POLARITY);
+
+	return 0;
+}
+
+int pd_tcpc_cc_nc(int port, int cc_volt, int cc_sel)
+{
+	int rp_index;
+	int nc;
+
+	/* Can never be called from CHG port as it's sink only */
+	if (port != DUT)
+		return 0;
+
+	rp_index = vbus_rp;
+	/*
+	 * If rp_index > 2, then always return not connected. This case should
+	 * only happen when all Rp GPIO controls are tri-stated.
+	 */
+	if (rp_index >= TYPEC_RP_RESERVED)
+		return 1;
+
+	/* Select the correct voltage threshold for current Rp and 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_config & CC_POLARITY ? !cc_sel : cc_sel];
+
+	return nc;
+}
+
+int pd_tcpc_cc_ra(int port, int cc_volt, int cc_sel)
+{
+	int rp_index;
+	int ra;
+
+	/* Can never be called from CHG port as it's sink only */
+	if (port != DUT)
+		return 0;
+
+	rp_index = vbus_rp;
+	/*
+	 * If rp_index > 2, then can't be Ra. This case should
+	 * only happen when all Rp GPIO controls are tri-stated.
+	 */
+	if (rp_index >= TYPEC_RP_RESERVED)
+		return 0;
+
+	/* Select the correct voltage threshold for current Rp and 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_config & CC_POLARITY ? !cc_sel : cc_sel];
+
+	return ra;
+}
+
+int pd_adc_read(int port, int cc)
+{
+	int mv = -1;
+
+	if (port == CHG)
+		mv = adc_read_channel(cc ? ADC_CHG_CC2_PD : ADC_CHG_CC1_PD);
+	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
+		 * (CC1 if polarity flip) and make the voltage high as vRd-3.0,
+		 * which makes the PD state mess up. As the PD state machine
+		 * doesn't handle this case. It assumes that CC2 (CC1 if
+		 * polarity flip) is separated by a Type-C cable, resulting a
+		 * voltage lower than the max of vRa.
+		 *
+		 * It fakes the voltage within vRa.
+		 */
+
+		/*
+		 * TODO: Fix this logic because of leakage "phantom detects"
+		 * Or flat-out mis-detects..... talking on leaking CC2 line.
+		 * And Vconn-swap case... and Ra on second line (SERVO_EMCA)...
+		 *
+		 * This is basically a hack faking "vOpen" from TCPCI spec.
+		 */
+		if ((cc_config & CC_DISABLE_DTS) &&
+		    port == DUT &&
+		    cc == ((cc_config & CC_POLARITY) ? 0 : 1)) {
+
+			if ((cc_pull_stored == TYPEC_CC_RD)  ||
+				(cc_pull_stored == TYPEC_CC_RA) ||
+				(cc_pull_stored == TYPEC_CC_RA_RD))
+				mv = -1;
+			else if (cc_pull_stored == TYPEC_CC_RP)
+				mv = 3301;
+		} else
+			mv = adc_read_channel(cc ? ADC_DUT_CC2_PD :
+						   ADC_DUT_CC1_PD);
+	} else {
+		/*
+		 * 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
+		 * alternatively that makes the voltage on CC falls into some
+		 * unexpected range and triggers the PD state machine switching
+		 * between SNK_DISCONNECTED and SNK_DISCONNECTED_DEBOUNCE.
+		 */
+		mv = -1;
+	}
+
+	return mv;
+}
+
+static int board_set_rp(int rp)
+{
+	if (cc_config & CC_DISABLE_DTS) {
+		/* TODO: Add SRC-EMCA mode (CC_EMCA_SERVO=1) */
+		/* TODO: Add SRC-nonEMCA mode (CC_EMCA_SERVO=0)*/
+
+		/*
+		 * DTS mode is disabled, so only present the requested Rp value
+		 * on CC1 (active) and leave all Rp/Rd resistors on CC2
+		 * (inactive) disconnected.
+		 */
+		switch (rp) {
+		case TYPEC_RP_USB:
+			DUT_ACTIVE_CC_PU(RPUSB);
+			break;
+		case TYPEC_RP_1A5:
+			DUT_ACTIVE_CC_PU(RP1A5);
+			break;
+		case TYPEC_RP_3A0:
+			DUT_ACTIVE_CC_PU(RP3A0);
+			break;
+		case TYPEC_RP_RESERVED:
+			/*
+			 * This case can be used to force a detach event since
+			 * all values are set to inputs above. Nothing else to
+			 * set.
+			 */
+			break;
+		default:
+			return EC_ERROR_INVAL;
+		}
+
+		/* TODO: Verify this (CC_EMCA_SERVO) statement works */
+		if (cc_config & CC_EMCA_SERVO)
+			DUT_INACTIVE_CC_PD(RA);
+		else
+			DUT_INACTIVE_CC_OPEN(RA);
+	} else {
+		/* DTS mode is enabled. The rp parameter is used to select the
+		 * Type C current limit to advertise. The combinations of Rp on
+		 * each CC line is shown in the table below.
+		 *
+		 * CC values for Debug sources (DTS)
+		 *
+		 * Source type  Mode of Operation   CC1    CC2
+		 * ---------------------------------------------
+		 * DTS          Default USB Power   Rp3A0  Rp1A5
+		 * DTS          USB-C @ 1.5 A       Rp1A5  RpUSB
+		 * DTS          USB-C @ 3 A         Rp3A0  RpUSB
+		 */
+		switch (rp) {
+		case TYPEC_RP_USB:
+			DUT_ACTIVE_CC_PU(RP3A0);
+			DUT_INACTIVE_CC_PU(RP1A5);
+			break;
+		case TYPEC_RP_1A5:
+			DUT_ACTIVE_CC_PU(RP1A5);
+			DUT_INACTIVE_CC_PU(RPUSB);
+			break;
+		case TYPEC_RP_3A0:
+			DUT_ACTIVE_CC_PU(RP3A0);
+			DUT_INACTIVE_CC_PU(RPUSB);
+			break;
+		case TYPEC_RP_RESERVED:
+			/*
+			 * This case can be used to force a detach event since
+			 * all values are set to inputs above. Nothing else to
+			 * set.
+			 */
+			break;
+		default:
+			return EC_ERROR_INVAL;
+		}
+	}
+	/* Save new Rp value for DUT port */
+	vbus_rp = rp;
+
+	return EC_SUCCESS;
+}
+
+int pd_set_rp_rd(int port, int cc_pull, int rp_value)
+{
+	int rv = EC_SUCCESS;
+
+	if (port != DUT)
+		return EC_ERROR_UNIMPLEMENTED;
+
+	/* CC is disabled for emulating detach. Don't change Rd/Rp. */
+	if (cc_config & CC_DETACH)
+		return EC_SUCCESS;
+
+	/* By default disconnect all Rp/Rd resistors from both CC lines */
+	/* Set Rd for CC1/CC2 to High-Z. */
+	DUT_BOTH_CC_OPEN(RD);
+	/* Set Ra for CC1/CC2 to High-Z. */
+	DUT_BOTH_CC_OPEN(RA);
+	/* Set Rp for CC1/CC2 to High-Z. */
+	DUT_BOTH_CC_OPEN(RP3A0);
+	DUT_BOTH_CC_OPEN(RP1A5);
+	DUT_BOTH_CC_OPEN(RPUSB);
+	/* Set TX Hi-Z */
+	DUT_BOTH_CC_OPEN(TX_DATA);
+
+	if (cc_pull == TYPEC_CC_RP) {
+		rv = board_set_rp(rp_value);
+	} else if ((cc_pull == TYPEC_CC_RD) || (cc_pull == TYPEC_CC_RA_RD) ||
+				(cc_pull == TYPEC_CC_RA)) {
+		/*
+		 * The DUT port uses a captive cable. It can present Rd on both
+		 * CC1 and CC2. If DTS mode is enabled, then present Rd on both
+		 * CC lines. However, if DTS mode is disabled only present Rd on
+		 * CC1 (active).
+		 *
+		 * TODO: EXCEPT if you have Ra_Rd or are "faking" an EMCA.....
+		 * ... or are applying RA+RA....can't make assumptions with
+		 * test equipment!
+		 */
+		if (cc_config & CC_DISABLE_DTS) {
+			if (cc_pull == TYPEC_CC_RD) {
+				DUT_ACTIVE_CC_PD(RD);
+				/*
+				 * TODO: Verify this (CC_EMCA_SERVO)
+				 * statement works
+				 */
+				if (cc_config & CC_EMCA_SERVO)
+					DUT_INACTIVE_CC_PD(RA);
+				else
+					DUT_INACTIVE_CC_OPEN(RA);
+			} else if (cc_pull == TYPEC_CC_RA) {
+				DUT_ACTIVE_CC_PD(RA);
+				/*
+				 * TODO: Verify this (CC_EMCA_SERVO)
+				 * statement works
+				 */
+				if (cc_config & CC_EMCA_SERVO)
+					DUT_INACTIVE_CC_PD(RA);
+				else
+					DUT_INACTIVE_CC_OPEN(RA);
+			} else if (cc_pull == TYPEC_CC_RA_RD) {
+				/*
+				 * TODO: Verify this silly (TYPEC_CC_RA_RD)
+				 * from TCPMv  works
+				 */
+				DUT_ACTIVE_CC_PD(RD);
+				DUT_INACTIVE_CC_PD(RA);
+			}
+		} else
+			DUT_BOTH_CC_PD(RD);
+
+		rv = EC_SUCCESS;
+	} else
+		return EC_ERROR_UNIMPLEMENTED;
+
+	rp_value_stored = rp_value;
+	cc_pull_stored = cc_pull;
+
+	return rv;
+}
+
+int board_select_rp_value(int port, int rp)
+{
+	if (port != DUT)
+		return EC_ERROR_UNIMPLEMENTED;
+
+	/*
+	 * Update Rp value to indicate non-pd power available.
+	 * Do not change pull direction though.
+	 */
+	if ((rp != rp_value_stored) && (cc_pull_stored == TYPEC_CC_RP)) {
+		rp_value_stored = rp;
+		return pd_set_rp_rd(port, TYPEC_CC_RP, rp);
+	}
+
+	return EC_SUCCESS;
+}
+
+int charge_manager_get_source_pdo(const uint32_t **src_pdo, const int port)
+{
+	int pdo_cnt = 0;
+
+	/*
+	 * If CHG is providing VBUS, then advertise what's available on the CHG
+	 * port, otherwise we provide no power.
+	 */
+	if (charge_port_is_active()) {
+		*src_pdo =  pd_src_chg_pdo;
+		pdo_cnt = chg_pdo_cnt;
+	}
+
+	return pdo_cnt;
+}
+
+__override void pd_transition_voltage(int idx)
+{
+	timestamp_t deadline;
+	uint32_t ma, mv;
+
+	pd_extract_pdo_power(pd_src_chg_pdo[idx - 1], &ma, &mv);
+	/* Is this a transition to a new voltage? */
+	if (charge_port_is_active() && vbus[CHG].mv != mv) {
+		/*
+		 * Alter voltage limit on charge port, this should cause
+		 * the port to select the desired PDO.
+		 */
+		pd_set_external_voltage_limit(CHG, mv);
+
+		/* Wait for CHG transition */
+		deadline.val = get_time().val + PD_T_PS_TRANSITION;
+		CPRINTS("Waiting for CHG port transition");
+		while (charge_port_is_active() &&
+		       vbus[CHG].mv != mv &&
+		       get_time().val < deadline.val)
+			msleep(10);
+
+		if (vbus[CHG].mv != mv) {
+			CPRINTS("Missed CHG transition, resetting DUT");
+			pd_power_supply_reset(DUT);
+			return;
+		}
+
+		CPRINTS("CHG transitioned");
+	}
+
+	vbus[DUT].mv = vbus[CHG].mv;
+	vbus[DUT].ma = vbus[CHG].ma;
+}
+
+int pd_set_power_supply_ready(int port)
+{
+	/* Port 0 can never provide vbus. */
+	if (port == CHG)
+		return EC_ERROR_INVAL;
+
+	if (charge_port_is_active()) {
+		/* Enable VBUS */
+		chg_power_select(CHG_POWER_VBUS);
+		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, get_dual_role_of_src());
+	} else {
+		vbus[DUT].mv = 0;
+		vbus[DUT].ma = 0;
+		dut_chg_en(0);
+		pd_set_dual_role(DUT, PD_DRP_FORCE_SINK);
+		return EC_ERROR_NOT_POWERED;
+	}
+
+	return EC_SUCCESS; /* we are ready */
+}
+
+void pd_power_supply_reset(int port)
+{
+	/* Port 0 can never provide vbus. */
+	if (port == CHG)
+		return;
+
+	/* Disable VBUS */
+	chg_power_select(CHG_POWER_OFF);
+	dut_chg_en(0);
+
+	/* DUT is lost, back to 5V limit on CHG */
+	pd_set_external_voltage_limit(CHG, PD_MIN_MV);
+}
+
+int pd_snk_is_vbus_provided(int port)
+{
+	return gpio_get_level(port ? GPIO_USB_DET_PP_DUT :
+				     GPIO_USB_DET_PP_CHG);
+}
+
+__override int pd_check_power_swap(int port)
+{
+	/*
+	 * When only host VBUS is available, then servo_v4 is not setting
+	 * PDO_FIXED_UNCONSTRAINED in the src_pdo sent to the DUT. When this bit
+	 * is not set, the DUT will always attempt to swap its power role to
+	 * 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;
+}
+
+__override int pd_check_data_swap(int port,
+				  enum pd_data_role data_role)
+{
+	/*
+	 * Servo should allow data role swaps to let DUT see the USB hub, but
+	 * doing it on CHG port is a waste as its data lines is unconnected.
+	 */
+	if (port == CHG)
+		return 0;
+
+	return 1;
+}
+
+__override void pd_execute_data_swap(int port,
+				     enum pd_data_role data_role)
+{
+	/*
+	 * TODO(b/137887386): Turn on the fastboot/DFU path when data swap to
+	 * DFP?
+	 */
+}
+
+__override void pd_check_pr_role(int port,
+				 enum pd_power_role pr_role,
+				 int flags)
+{
+	/*
+	 * Don't define any policy to initiate power role swap.
+	 *
+	 * CHG port is SNK only. DUT port requires a user to switch its
+	 * role by commands. So don't do anything implicitly.
+	 */
+}
+
+__override void pd_check_dr_role(int port,
+				 enum pd_data_role dr_role,
+				 int flags)
+{
+	if (port == CHG)
+		return;
+
+	/* If DFP, try to switch to UFP, to let DUT see the USB hub. */
+	if ((flags & PD_FLAGS_PARTNER_DR_DATA) && dr_role == PD_ROLE_DFP)
+		pd_request_data_swap(port);
+}
+
+
+/* ----------------- Vendor Defined Messages ------------------ */
+
+const uint32_t vdo_idh = VDO_IDH(0, /* data caps as USB host */
+				 1, /* data caps as USB device */
+				 IDH_PTYPE_AMA, /* Alternate mode */
+				 1, /* supports alt modes */
+				 USB_VID_GOOGLE);
+
+const uint32_t vdo_product = VDO_PRODUCT(CONFIG_USB_PID, CONFIG_USB_BCD_DEV);
+
+const uint32_t vdo_ama = VDO_AMA(CONFIG_USB_PD_IDENTITY_HW_VERS,
+				 CONFIG_USB_PD_IDENTITY_SW_VERS,
+				 0, 0, 0, 0, /* SS[TR][12] */
+				 0, /* Vconn power */
+				 0, /* Vconn power required */
+				 0, /* Vbus power required */
+				 AMA_USBSS_U31_GEN1 /* USB SS support */);
+
+static int svdm_response_identity(int port, uint32_t *payload)
+{
+	/*
+	 * TODO(b/137219603): Make whether servo supports DP alt-mode
+	 * configurable, like through a console command.
+	 *
+	 * This version is to check if a monitor is plugged to the mini-DP port
+	 * to decide if DP alt-mode is supported or not. So no alt-mode
+	 * supported if no monitor is plugged before plugging the servo Type-C
+	 * cable to DUT. This way doesn't affect PD FAFT results.
+	 */
+	int dp_supported = gpio_get_level(GPIO_DP_HPD);
+
+	if (dp_supported) {
+		payload[VDO_I(IDH)] = vdo_idh;
+		payload[VDO_I(CSTAT)] = VDO_CSTAT(0);
+		payload[VDO_I(PRODUCT)] = vdo_product;
+		payload[VDO_I(AMA)] = vdo_ama;
+		return VDO_I(AMA) + 1;
+	} else {
+		return 0;
+	}
+}
+
+static int svdm_response_svids(int port, uint32_t *payload)
+{
+	payload[1] = VDO_SVID(USB_SID_DISPLAYPORT, 0);
+	return 2;
+}
+
+#define MODE_CNT 1
+#define OPOS 1
+
+/*
+ * The Type-C demux TUSB1064 supports pin assignment C and D. Response the DP
+ * capabilities with supporting all of them.
+ *
+ * TODO(b/137219603): Make this pin assignment and plug/receptacle configurable
+ * by a console command that some tests can check different dongle behaviors.
+ */
+const uint32_t vdo_dp_mode[MODE_CNT] =  {
+	VDO_MODE_DP(0,             /* UFP pin cfg supported: none */
+		    MODE_DP_PIN_C | MODE_DP_PIN_D | MODE_DP_PIN_E, /* DFP pin */
+		    1,             /* no usb2.0 signalling in AMode */
+		    CABLE_PLUG,    /* Its a plug */
+		    MODE_DP_V13,   /* DPv1.3 Support, no Gen2 */
+		    MODE_DP_SNK)   /* Its a sink only */
+};
+
+static int svdm_response_modes(int port, uint32_t *payload)
+{
+
+	/* CCD uses the SBU lines; don't enable DP when dts-mode enabled */
+	if (!(cc_config & CC_DISABLE_DTS))
+		return 0; /* NAK */
+
+	if (PD_VDO_VID(payload[0]) != USB_SID_DISPLAYPORT)
+		return 0; /* NAK */
+
+	memcpy(payload + 1, vdo_dp_mode, sizeof(vdo_dp_mode));
+	return MODE_CNT + 1;
+}
+
+static int is_typec_dp_muxed(void)
+{
+	int value;
+
+	i2c_read8(I2C_PORT_MASTER, TUSB1064_I2C_ADDR_FLAG, TUSB1064_REG_GENERAL,
+		&value);
+	return value & TUSB1064_MODE_DP_ENABLED ? 1 : 0;
+}
+
+static void set_typec_mux(int pin_cfg)
+{
+	int value;
+
+	i2c_read8(I2C_PORT_MASTER, TUSB1064_I2C_ADDR_FLAG, TUSB1064_REG_GENERAL,
+		&value);
+	value &= ~(TUSB1064_MODE_DP_ENABLED | TUSB1064_MODE_USB_ENABLED);
+	switch (pin_cfg) {
+	case 0:
+		CPRINTS("PinCfg:off");
+		break;
+	case MODE_DP_PIN_C:
+		value |= TUSB1064_MODE_DP_ENABLED;
+		CPRINTS("PinCfg:C");
+		break;
+	case MODE_DP_PIN_D:
+		value |= TUSB1064_MODE_DP_ENABLED | TUSB1064_MODE_USB_ENABLED;
+		CPRINTS("PinCfg:D");
+		break;
+	default:
+		CPRINTS("PinCfg not supported: %d", pin_cfg);
+		return;
+	}
+	if (value && cc_config & CC_POLARITY)
+		value |= TUSB1064_MODE_POLARITY_INVERTED;
+	else
+		value &= ~TUSB1064_MODE_POLARITY_INVERTED;
+
+	i2c_write8(I2C_PORT_MASTER, TUSB1064_I2C_ADDR_FLAG,
+		TUSB1064_REG_GENERAL, value);
+}
+
+static int dp_status(int port, uint32_t *payload)
+{
+	int opos = PD_VDO_OPOS(payload[0]);
+	int hpd = gpio_get_level(GPIO_DP_HPD);
+
+	if (opos != OPOS)
+		return 0;  /* NAK */
+
+	/*
+	 * TODO(b/137219603): Make the Multi-Function Preferred bit
+	 * configurable by a console command.
+	 */
+	payload[1] = VDO_DP_STATUS(0,                /* IRQ_HPD */
+				   hpd,              /* HPD_HI|LOW */
+				   0,                /* request exit DP */
+				   0,                /* request exit USB */
+				   1,                /* MF pref */
+				   is_typec_dp_muxed(),
+				   0,                /* power low */
+				   0x2);
+	return 2;
+}
+
+static int dp_config(int port, uint32_t *payload)
+{
+	if (PD_DP_CFG_DPON(payload[1]))
+		set_typec_mux(PD_DP_CFG_PIN(payload[1]));
+
+	return 1;
+}
+
+/* Whether alternate mode has been entered or not */
+static int alt_mode;
+
+static int svdm_enter_mode(int port, uint32_t *payload)
+{
+	/* SID & mode request is valid */
+	if ((PD_VDO_VID(payload[0]) != USB_SID_DISPLAYPORT) ||
+	    (PD_VDO_OPOS(payload[0]) != OPOS))
+		return 0;  /* NAK */
+
+	alt_mode = OPOS;
+	return 1;
+}
+
+int pd_alt_mode(int port, enum tcpm_transmit_type type, uint16_t svid)
+{
+	if (type != TCPC_TX_SOP)
+		return 0;
+
+	if (svid == USB_SID_DISPLAYPORT)
+		return alt_mode;
+
+	return 0;
+}
+
+static int svdm_exit_mode(int port, uint32_t *payload)
+{
+	if (PD_VDO_VID(payload[0]) == USB_SID_DISPLAYPORT)
+		set_typec_mux(0);
+
+	alt_mode = 0;
+
+	return 1; /* Must return ACK */
+}
+
+static struct amode_fx dp_fx = {
+	.status = &dp_status,
+	.config = &dp_config,
+};
+
+const struct svdm_response svdm_rsp = {
+	.identity = &svdm_response_identity,
+	.svids = &svdm_response_svids,
+	.modes = &svdm_response_modes,
+	.enter_mode = &svdm_enter_mode,
+	.amode = &dp_fx,
+	.exit_mode = &svdm_exit_mode,
+};
+
+__override int pd_custom_vdm(int port, int cnt, uint32_t *payload,
+		  uint32_t **rpayload)
+{
+	int cmd = PD_VDO_CMD(payload[0]);
+
+	/* make sure we have some payload */
+	if (cnt == 0)
+		return 0;
+
+	switch (cmd) {
+	case VDO_CMD_VERSION:
+		/* guarantee last byte of payload is null character */
+		*(payload + cnt - 1) = 0;
+		CPRINTF("ver: %s\n", (char *)(payload+1));
+		break;
+	case VDO_CMD_CURRENT:
+		CPRINTF("Current: %dmA\n", payload[1]);
+		break;
+	}
+
+	return 0;
+}
+
+__override const struct svdm_amode_fx supported_modes[] = {};
+__override const int supported_modes_cnt = ARRAY_SIZE(supported_modes);
+
+static void print_cc_mode(void)
+{
+	/* Get current CCD status */
+	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",
+		 get_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");
+	ccprintf("cc polarity: %s\n", cc_config & CC_POLARITY ? "cc2" :
+								"cc1");
+	ccprintf("pd enabled: %s\n", pd_comm_is_enabled(DUT) ? "on" : "off");
+	ccprintf("emca: %s\n", cc_config & CC_EMCA_SERVO ?
+					"emarked" : "non-emarked");
+}
+
+
+static void do_cc(int cc_config_new)
+{
+	int chargeable;
+	int dualrole;
+
+	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 */
+			cc_config &= ~(CC_DISABLE_DTS & CC_ALLOW_SRC);
+
+			/* Remove Rp/Rd on both CC lines */
+			pd_comm_enable(DUT, 0);
+			pd_set_rp_rd(DUT, TYPEC_CC_RP, TYPEC_RP_RESERVED);
+
+			/*
+			 * If just changing mode (cc keeps enabled), give some
+			 * time for DUT to detach, use tErrorRecovery.
+			 */
+			if (!(cc_config_new & CC_DETACH))
+				usleep(PD_T_ERROR_RECOVERY);
+		}
+
+		if ((cc_config & ~cc_config_new) & CC_DISABLE_DTS) {
+			/* DTS-disabled -> DTS-enabled */
+			ccd_enable(1);
+		} else if ((cc_config_new & ~cc_config) & CC_DISABLE_DTS) {
+			/* DTS-enabled -> DTS-disabled */
+			ccd_enable(0);
+		}
+
+		/* Accept new cc_config value */
+		cc_config = cc_config_new;
+
+		if (!(cc_config & CC_DETACH)) {
+			/* Can we source? */
+			chargeable = is_charge_through_allowed();
+			dualrole = chargeable ? get_dual_role_of_src() :
+						PD_DRP_FORCE_SINK;
+			pd_set_dual_role(DUT, dualrole);
+			/*
+			 * 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.
+			 */
+			if (dualrole != PD_DRP_TOGGLE_ON)
+				pd_set_host_mode(DUT, chargeable);
+
+			/*
+			 * For the normal lab use, emulating a sink has no PD
+			 * comm, like a passive hub. For the PD FAFT use, we
+			 * need to validate some PD behavior, so a flag
+			 * CC_SNK_WITH_PD to force enabling PD comm.
+			 */
+			if (cc_config & CC_SNK_WITH_PD)
+				pd_comm_enable(DUT, 1);
+			else
+				pd_comm_enable(DUT, chargeable);
+		}
+	}
+}
+
+static int command_cc(int argc, char **argv)
+{
+	int cc_config_new = cc_config;
+
+	if (argc < 2) {
+		print_cc_mode();
+		return EC_SUCCESS;
+	}
+
+	if (!strcasecmp(argv[1], "off")) {
+		cc_config_new |= CC_DETACH;
+	} else if (!strcasecmp(argv[1], "on")) {
+		cc_config_new &= ~CC_DETACH;
+	} else {
+		cc_config_new &= ~CC_DETACH;
+		if (!strcasecmp(argv[1], "src"))
+			cc_config_new = CONF_SRC(cc_config_new);
+		else if (!strcasecmp(argv[1], "snk"))
+			cc_config_new = CONF_SNK(cc_config_new);
+		else if (!strcasecmp(argv[1], "pdsnk"))
+			cc_config_new = CONF_PDSNK(cc_config_new);
+		else if (!strcasecmp(argv[1], "drp"))
+			cc_config_new = CONF_DRP(cc_config_new);
+		else if (!strcasecmp(argv[1], "srcdts"))
+			cc_config_new = CONF_SRCDTS(cc_config_new);
+		else if (!strcasecmp(argv[1], "snkdts"))
+			cc_config_new = CONF_SNKDTS(cc_config_new);
+		else if (!strcasecmp(argv[1], "pdsnkdts"))
+			cc_config_new = CONF_PDSNKDTS(cc_config_new);
+		else if (!strcasecmp(argv[1], "drpdts"))
+			cc_config_new = CONF_DRPDTS(cc_config_new);
+		else if (!strcasecmp(argv[1], "emca"))
+			cc_config_new |= CC_EMCA_SERVO;
+		else if (!strcasecmp(argv[1], "nonemca"))
+			cc_config_new &= ~CC_EMCA_SERVO;
+		else
+			return EC_ERROR_PARAM2;
+	}
+
+	if (!strcasecmp(argv[2], "cc1"))
+		cc_config_new &= ~CC_POLARITY;
+	else if (!strcasecmp(argv[2], "cc2"))
+		cc_config_new |= CC_POLARITY;
+	else if (argc >= 3)
+		return EC_ERROR_PARAM3;
+
+	do_cc(cc_config_new);
+	print_cc_mode();
+
+	return EC_SUCCESS;
+}
+DECLARE_CONSOLE_COMMAND(cc, command_cc,
+			"[off|on|src|snk|pdsnk|drp|srcdts|snkdts|pdsnkdts|"
+			"drpdts|emca|nonemca] [cc1|cc2]",
+			"Servo_v4 DTS and CHG mode");
+
+static void fake_disconnect_end(void)
+{
+	/* Reenable CC lines with previous dts and src modes */
+	do_cc(cc_config & ~CC_DETACH);
+}
+DECLARE_DEFERRED(fake_disconnect_end);
+
+static void fake_disconnect_start(void)
+{
+	/* Disable CC lines */
+	do_cc(cc_config | CC_DETACH);
+
+	hook_call_deferred(&fake_disconnect_end_data,
+			   fake_pd_disconnect_duration_us);
+}
+DECLARE_DEFERRED(fake_disconnect_start);
+
+static int cmd_fake_disconnect(int argc, char *argv[])
+{
+	int delay_ms, duration_ms;
+	char *e;
+
+	if (argc < 3)
+		return EC_ERROR_PARAM_COUNT;
+
+	delay_ms = strtoi(argv[1], &e, 0);
+	if (*e || delay_ms < 0)
+		return EC_ERROR_PARAM1;
+	duration_ms = strtoi(argv[2], &e, 0);
+	if (*e || duration_ms < 0)
+		return EC_ERROR_PARAM2;
+
+	/* Cancel any pending function calls */
+	hook_call_deferred(&fake_disconnect_start_data, -1);
+	hook_call_deferred(&fake_disconnect_end_data, -1);
+
+	fake_pd_disconnect_duration_us = duration_ms * MSEC;
+	hook_call_deferred(&fake_disconnect_start_data, delay_ms * MSEC);
+
+	ccprintf("Fake disconnect for %d ms starting in %d ms.\n",
+		duration_ms, delay_ms);
+
+	return EC_SUCCESS;
+}
+DECLARE_CONSOLE_COMMAND(fakedisconnect, cmd_fake_disconnect,
+			"<delay_ms> <duration_ms>", NULL);
+
+static int cmd_ada_srccaps(int argc, char *argv[])
+{
+	int i;
+	const uint32_t * const ada_srccaps = pd_get_src_caps(CHG);
+
+	for (i = 0; i < pd_get_src_cap_cnt(CHG); ++i) {
+		uint32_t max_ma, max_mv;
+
+		pd_extract_pdo_power(ada_srccaps[i], &max_ma, &max_mv);
+		ccprintf("%d: %dmV/%dmA\n", i, max_mv, max_ma);
+	}
+
+	return EC_SUCCESS;
+}
+DECLARE_CONSOLE_COMMAND(ada_srccaps, cmd_ada_srccaps,
+			"",
+			"Print adapter SrcCap");
+
+static int cmd_usbc_action(int argc, char *argv[])
+{
+	if (argc != 2 && argc != 3)
+		return EC_ERROR_PARAM_COUNT;
+
+	/* TODO(b:140256624): drop *v command if we migrate to chg cmd. */
+	if (!strcasecmp(argv[1], "5v")) {
+		do_cc(CONF_SRC(cc_config));
+		user_limited_max_mv = 5000;
+		update_ports();
+	} else if (!strcasecmp(argv[1], "12v")) {
+		do_cc(CONF_SRC(cc_config));
+		user_limited_max_mv = 12000;
+		update_ports();
+	} else if (!strcasecmp(argv[1], "20v")) {
+		do_cc(CONF_SRC(cc_config));
+		user_limited_max_mv = 20000;
+		update_ports();
+	} else if (!strcasecmp(argv[1], "dev")) {
+		/* Set the limit back to original */
+		user_limited_max_mv = 20000;
+		do_cc(CONF_PDSNK(cc_config));
+	} else if (!strcasecmp(argv[1], "pol0")) {
+		do_cc(cc_config & ~CC_POLARITY);
+	} else if (!strcasecmp(argv[1], "pol1")) {
+		do_cc(cc_config | CC_POLARITY);
+	} else 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 if (!strcasecmp(argv[1], "chg")) {
+		int sink_v;
+
+		if (argc != 3)
+			return EC_ERROR_PARAM2;
+
+		sink_v = atoi(argv[2]);
+		if (!sink_v)
+			return EC_ERROR_PARAM2;
+
+		user_limited_max_mv = sink_v * 1000;
+		do_cc(CONF_SRC(cc_config));
+		update_ports();
+		/*
+		 * TODO(b:140256624): servod captures 'chg SRC' keyword to
+		 * recognize if this command is supported in the firmware.
+		 * Drop this message if when we phase out the usbc_role control.
+		 */
+		ccprintf("CHG SRC %dmV\n", user_limited_max_mv);
+	} else {
+		return EC_ERROR_PARAM1;
+	}
+
+	return EC_SUCCESS;
+}
+DECLARE_CONSOLE_COMMAND(usbc_action, cmd_usbc_action,
+			"5v|12v|20v|dev|pol0|pol1|drp|chg x(x=voltage)",
+			"Set Servo v4 type-C port state");