servo_v4: Added initial USB PD support for both CHG/DUT ports

- CHG port can connect as SNK at different voltage levels
- DUT port presents as SNK only
- DUT port uses fixed polarity since it has a fixed cable
- Not supporting ALT or ALT_DP modes in terms of svdm messages at
  this point.
- No support yet for USB mux.

BUG=chromium:571476
BRANCH=None
TEST=Manual
CHG port: Tested with Zinger and Plankton and 5/12/20V VBUS levels.
DUT port: Tested against Reef and verified that port reached SNK_READY.

Change-Id: Ib645872790912f9e0a0d4adddc10345a59145d3e
Signed-off-by: Scott <scollyer@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/424413
Commit-Ready: Scott Collyer <scollyer@chromium.org>
Tested-by: Scott Collyer <scollyer@chromium.org>
Reviewed-by: Nick Sanders <nsanders@chromium.org>
Reviewed-by: Shawn N <shawnn@chromium.org>

7 files changed, 594 insertions, 11 deletions

diff --git a/board/servo_v4/board.c b/board/servo_v4/board.c
index eb884b4822..9fb6fbd502 100644
--- a/board/servo_v4/board.c
+++ b/board/servo_v4/board.c
@@ -27,9 +27,42 @@
 #include "usb-stream.h"
 #include "util.h"
 
-
 #define CPRINTS(format, args...) cprints(CC_SYSTEM, format, ## args)
 
+/******************************************************************************
+ * Board pre-init function.
+ */
+
+void board_config_pre_init(void)
+{
+	/* enable SYSCFG clock */
+	STM32_RCC_APB2ENR |= 1 << 0;
+
+	/*
+	 * the DMA mapping is :
+	 *  Chan 2 : TIM1_CH1  (CHG RX) - Default mapping
+	 *  Chan 3 : SPI1_TX   (CHG TX) - Default mapping
+	 *  Chan 4 : USART1 TX - Remapped from default Chan 2
+	 *  Chan 5 : USART1 RX - Remapped from default Chan 3
+	 *  Chan 6 : TIM3_CH1  (DUT RX) - Remapped from default Chan 4
+	 *  Chan 7 : SPI2_TX   (DUT TX) - Remapped from default Chan 5
+	 *
+	 * As described in the comments above, both USART1 TX/RX and DUT Tx/RX
+	 * channels must be remapped from the defulat locations. Remapping is
+	 * acoomplished by setting the following bits in the STM32_SYSCFG_CFGR1
+	 * register. Information about this register and its settings can be
+	 * found in section 11.3.7 DMA Request Mapping of the STM RM0091
+	 * Reference Manual
+	 */
+	/* Remap USART1 Tx from DMA channel 2 to channel 4 */
+	STM32_SYSCFG_CFGR1 |= (1 << 9);
+	/* Remap USART1 Rx from DMA channel 3 to channel 5 */
+	STM32_SYSCFG_CFGR1 |= (1 << 10);
+	/* Remap TIM3_CH1 from DMA channel 4 to channel 6 */
+	STM32_SYSCFG_CFGR1 |= (1 << 30);
+	/* Remap SPI2 Tx from DMA channel 5 to channel 7 */
+	STM32_SYSCFG_CFGR1 |= (1 << 24);
+}
 
 /******************************************************************************
  * Build GPIO tables and expose a subset of the GPIOs over USB.
@@ -92,10 +125,10 @@ USB_GPIO_CONFIG(usb_gpio,
 /* ADC channels */
 const struct adc_t adc_channels[] = {
 	/* USB PD CC lines sensing. Converted to mV (3300mV/4096). */
-	[ADC_DUT_CC1_PD] = {"DUT_CC1_PD", 3300, 4096, 0, STM32_AIN(0)},
-	[ADC_DUT_CC2_PD] = {"DUT_CC2_PD", 3300, 4096, 0, STM32_AIN(5)},
 	[ADC_CHG_CC1_PD] = {"CHG_CC1_PD", 3300, 4096, 0, STM32_AIN(2)},
 	[ADC_CHG_CC2_PD] = {"CHG_CC2_PD", 3300, 4096, 0, STM32_AIN(4)},
+	[ADC_DUT_CC1_PD] = {"DUT_CC1_PD", 3300, 4096, 0, STM32_AIN(0)},
+	[ADC_DUT_CC2_PD] = {"DUT_CC2_PD", 3300, 4096, 0, STM32_AIN(5)},
 	[ADC_SBU1_DET] = {"SBU1_DET", 3300, 4096, 0, STM32_AIN(3)},
 	[ADC_SBU2_DET] = {"SBU2_DET", 3300, 4096, 0, STM32_AIN(7)},
 	[ADC_SUB_C_REF] = {"SUB_C_REF", 3300, 4096, 0, STM32_AIN(1)},
@@ -474,6 +507,12 @@ static void board_init(void)
 	init_ioexpander();
 	init_uservo_port();
 
+	/*
+	 * TODO(crosbug.com/p/60828): The result of init_ccd() will be
+	 * overwritten when the usb pd protocol state machine attempts to attach
+	 * as SNK or SRC since it will modify the pullup/pulldown resistor on
+	 * the chosen polarity CC line.
+	 */
 	/* Enable CCD if type-c */
 	if (gpio_get_level(GPIO_DONGLE_DET))
 		init_ccd(CCD_ID_RPUSB);
diff --git a/board/servo_v4/board.h b/board/servo_v4/board.h
index 24cff51544..38c7249375 100644
--- a/board/servo_v4/board.h
+++ b/board/servo_v4/board.h
@@ -70,6 +70,40 @@
 
 /* PD features */
 #define CONFIG_ADC
+#define CONFIG_BOARD_PRE_INIT
+/*
+ * If task profiling is enabled then the rx falling edge detection interrupts
+ * can't be processed in time and can't support USB PD messaging.
+ */
+#undef CONFIG_TASK_PROFILING
+
+#define CONFIG_USB_POWER_DELIVERY
+#define CONFIG_USB_PD_DUAL_ROLE
+#define CONFIG_USB_PD_INTERNAL_COMP
+#define CONFIG_USB_PD_PORT_COUNT 2
+#define CONFIG_USB_PD_TCPC
+#define CONFIG_USB_PD_TCPM_STUB
+
+/* 3.0A Standard-current Rp */
+#define PD_SRC_VNC           PD_SRC_DEF_VNC_MV
+#define PD_SRC_RD_THRESHOLD  PD_SRC_DEF_RD_THRESH_MV
+
+/* Start as a sink for both CHG/DUT ports */
+#define PD_DEFAULT_STATE PD_STATE_SNK_DISCONNECTED
+
+/*
+ * 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
+ * be supplied to the DUT port from the CHG port.
+ */
+#define PD_POWER_SUPPLY_TURN_ON_DELAY  50000  /* us */
+#define PD_POWER_SUPPLY_TURN_OFF_DELAY 50000 /* us */
+
+/* Define typical operating power and max power */
+#define PD_OPERATING_POWER_MW 15000
+#define PD_MAX_POWER_MW       60000
+#define PD_MAX_CURRENT_MA     3000
+#define PD_MAX_VOLTAGE_MV     20000
 
 /*
  * Allow dangerous commands all the time, since we don't have a write protect
@@ -98,17 +132,16 @@ enum usb_strings {
 	USB_STR_USART3_STREAM_NAME,
 	USB_STR_USART4_STREAM_NAME,
 	USB_STR_UPDATE_NAME,
-
 	USB_STR_COUNT
 };
 
 
 /* ADC signal */
 enum adc_channel {
-	ADC_DUT_CC1_PD = 0,
-	ADC_DUT_CC2_PD,
-	ADC_CHG_CC1_PD,
+	ADC_CHG_CC1_PD = 0,
 	ADC_CHG_CC2_PD,
+	ADC_DUT_CC1_PD,
+	ADC_DUT_CC2_PD,
 	ADC_SBU1_DET,
 	ADC_SBU2_DET,
 	ADC_SUB_C_REF,
diff --git a/board/servo_v4/build.mk b/board/servo_v4/build.mk
index 0e069a31ad..be86135868 100644
--- a/board/servo_v4/build.mk
+++ b/board/servo_v4/build.mk
@@ -11,3 +11,6 @@ CHIP_FAMILY:=stm32f0
 CHIP_VARIANT:=stm32f07x
 
 board-y=board.o
+board-$(CONFIG_USB_POWER_DELIVERY)+=usb_pd_policy.o
+
+all_deps=$(patsubst ro,,$(def_all_deps))
diff --git a/board/servo_v4/ec.tasklist b/board/servo_v4/ec.tasklist
index cd197cfd36..796eb96f7a 100644
--- a/board/servo_v4/ec.tasklist
+++ b/board/servo_v4/ec.tasklist
@@ -18,4 +18,6 @@
  */
 #define CONFIG_TASK_LIST \
 	TASK_ALWAYS(HOOKS,   hook_task,    NULL, VENTI_TASK_STACK_SIZE) \
-	TASK_ALWAYS(CONSOLE, console_task, NULL, TASK_STACK_SIZE)
+	TASK_ALWAYS(CONSOLE, console_task, NULL, TASK_STACK_SIZE) \
+        TASK_ALWAYS(PD_C0, pd_task, NULL, LARGER_TASK_STACK_SIZE) \
+        TASK_ALWAYS(PD_C1, pd_task, NULL, LARGER_TASK_STACK_SIZE)
diff --git a/board/servo_v4/gpio.inc b/board/servo_v4/gpio.inc
index ab146777d9..b0d8f124f1 100644
--- a/board/servo_v4/gpio.inc
+++ b/board/servo_v4/gpio.inc
@@ -47,7 +47,7 @@ GPIO(USB_DUT_CC2_TX_DATA,	PIN(C, 2),  GPIO_INPUT)
 GPIO(USB_DUT_CC1_RP3A0,		PIN(C, 0),  GPIO_INPUT)
 GPIO(USB_DUT_CC1_RP1A5,		PIN(C, 1),  GPIO_INPUT)
 GPIO(USB_DUT_CC1_RPUSB,		PIN(C, 3),  GPIO_INPUT)
-GPIO(USB_DUT_CC1_RD,		PIN(C, 6),  GPIO_OUT_LOW)
+GPIO(USB_DUT_CC1_RD,		PIN(C, 6),  GPIO_INPUT)
 GPIO(USB_DUT_CC1_RA,		PIN(C, 7),  GPIO_INPUT)
 
 GPIO(USB_DUT_CC2_RP3A0,		PIN(C, 8),  GPIO_INPUT)
@@ -62,7 +62,6 @@ GPIO(USB_CHG_TX_CLKIN,        PIN(B, 3),  GPIO_INPUT)
 GPIO(USB_DUT_TX_CLKOUT,       PIN(B, 15), GPIO_INPUT)
 GPIO(USB_DUT_TX_CLKIN,        PIN(B, 13), GPIO_INPUT)
 
-
 /* I2C pins should be configured as inputs until I2C module is    */
 /* initialized. This will avoid driving the lines unintentionally.*/
 GPIO(MASTER_I2C_SCL,		PIN(B, 10), GPIO_INPUT)
@@ -75,7 +74,6 @@ UNIMPLEMENTED(WP_L)
 ALTERNATE(PIN_MASK(C, 0x0030), 1, MODULE_USART, 0) /* USART3: PC4/PC5 - Servo DUT UART */
 ALTERNATE(PIN_MASK(C, 0x0C00), 0, MODULE_USART, 0) /* USART4: PC10/PC11 - Servo UART3 */
 ALTERNATE(PIN_MASK(B, 0x0C00), 1, MODULE_I2C, GPIO_ODR_HIGH) /* I2C MASTER:PB8/9 */
-
 ALTERNATE(PIN_MASK(B, 0x0008), 0, MODULE_USB_PD, 0) /* SPI1_SCK:  PB3   */
 ALTERNATE(PIN_MASK(B, 0x2000), 0, MODULE_USB_PD, 0) /* SPI2_SCK:  PB13  */
 ALTERNATE(PIN_MASK(B, 0x0100), 2, MODULE_USB_PD, 0) /* TIM16_CH1: PB8   */
diff --git a/board/servo_v4/usb_pd_config.h b/board/servo_v4/usb_pd_config.h
new file mode 100644
index 0000000000..80e7bb7388
--- /dev/null
+++ b/board/servo_v4/usb_pd_config.h
@@ -0,0 +1,324 @@
+/* Copyright 2017 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"
+
+/* USB Power delivery board configuration */
+
+#ifndef __CROS_EC_USB_PD_CONFIG_H
+#define __CROS_EC_USB_PD_CONFIG_H
+
+/*
+ * For DUT PD port uses a fixed cable and both CC lines are connected. Need to
+ * choose a default polarity. DUT_CC_POLARITY is this selection.
+ */
+#define DUT_CC_POLARITY 0
+
+
+/* NOTES: Servo V4 and glados equivalents:
+ *	Glados		Servo V4
+ *	C0		CHG
+ *	C1		DUT
+ *
+ */
+
+/* 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
+
+/* 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) : (1 << 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 |= (1 << 12);
+		STM32_RCC_APB2RSTR &= ~(1 << 12);
+	} else {
+		/* Reset SPI2 */
+		STM32_RCC_APB1RSTR |= (1 << 14);
+		STM32_RCC_APB1RSTR &= ~(1 << 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_PD, 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_PD, 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_PD, 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_PD, 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_PD, GPIO_ANALOG);
+		} else {
+			gpio_set_flags(GPIO_USB_CHG_CC1_TX_DATA, GPIO_INPUT);
+			gpio_set_flags(GPIO_USB_CHG_CC1_PD, GPIO_ANALOG);
+		}
+	} else {
+		if (polarity) {
+			gpio_set_flags(GPIO_USB_DUT_CC2_TX_DATA, GPIO_INPUT);
+			gpio_set_flags(GPIO_USB_DUT_CC2_PD, GPIO_ANALOG);
+		} else {
+			gpio_set_flags(GPIO_USB_DUT_CC1_TX_DATA, GPIO_INPUT);
+			gpio_set_flags(GPIO_USB_DUT_CC1_PD, 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)
+{
+	if (port == 0) {
+		/* CHG port. Rd is wired to 5k pulldown. */
+		/* Can't set host mode. */
+	} else {
+		if (enable) {
+			/*
+			 * TODO(crosbug.com/p/60792): Similar to what's done for
+			 * SNK mode, only want to adjust the CC line that
+			 * matches the chosen polarity for the DUT port. Since
+			 * DUT port uses a fixed cable and both CC lines are
+			 * connected will only need to toggle 1 of the CC lines.
+			 */
+			/* High-Z is used for host mode. */
+			gpio_set_flags(GPIO_USB_DUT_CC1_RD, GPIO_INPUT);
+			gpio_set_flags(GPIO_USB_DUT_CC2_RD, GPIO_INPUT);
+			/* Pull up for host mode */
+			gpio_set_flags(GPIO_USB_DUT_CC1_RP1A5, GPIO_OUTPUT);
+			gpio_set_level(GPIO_USB_DUT_CC1_RP1A5, 1);
+			gpio_set_flags(GPIO_USB_DUT_CC2_RP1A5, GPIO_OUTPUT);
+			gpio_set_level(GPIO_USB_DUT_CC2_RP1A5, 1);
+			/* Set TX Hi-Z */
+			gpio_set_flags(GPIO_USB_DUT_CC1_TX_DATA, GPIO_INPUT);
+			gpio_set_flags(GPIO_USB_DUT_CC2_TX_DATA, GPIO_INPUT);
+		} else {
+			/* Set RD to High-Z for device mode. */
+			gpio_set_flags(GPIO_USB_DUT_CC1_RP1A5, GPIO_INPUT);
+			gpio_set_flags(GPIO_USB_DUT_CC2_RP1A5, GPIO_INPUT);
+			/*
+			 * TODO(crosbug.com/p/60828): Undo what was done in
+			 * init_ccd.
+			 */
+			gpio_set_flags(GPIO_USB_DUT_CC1_RPUSB, GPIO_INPUT);
+			gpio_set_flags(GPIO_USB_DUT_CC2_RPUSB, GPIO_INPUT);
+			if (DUT_CC_POLARITY == 0) {
+				gpio_set_flags(GPIO_USB_DUT_CC1_RD,
+					       GPIO_OUTPUT);
+				gpio_set_level(GPIO_USB_DUT_CC1_RD, 0);
+			} else {
+				gpio_set_flags(GPIO_USB_DUT_CC2_RD,
+					       GPIO_OUTPUT);
+				gpio_set_level(GPIO_USB_DUT_CC2_RD, 0);
+			}
+		}
+	}
+}
+
+/**
+ * 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, and charge_en and vbus_en GPIOs to match
+	 * the initial role.
+	 */
+	pd_set_host_mode(port, power_role);
+
+	/* Initialize TX pins and put them in Hi-Z */
+	pd_tx_init();
+
+}
+
+static inline int pd_adc_read(int port, int cc)
+{
+	int mv;
+
+	if (port == 0)
+		mv = adc_read_channel(cc ? ADC_CHG_CC2_PD : ADC_CHG_CC1_PD);
+	else {
+		/*
+		 * TODO(crosbug.com/p/60792): Only want to read the CC line that
+		 * matches the chosen polarity for DUT port. If not for the
+		 * chosen polarity, then return 0. This only works when the DUT
+		 * port is presenting as a SNK device. Need to fix this so that
+		 * the returned value for the cc line that doesn't match the
+		 * chosen polarity returns 0 for SNK and 3300 for SRC mode.
+		 */
+		if (cc == DUT_CC_POLARITY)
+			mv = adc_read_channel(cc ? ADC_DUT_CC2_PD :
+						ADC_DUT_CC1_PD);
+		else
+			mv = 0;
+	}
+
+	return mv;
+}
+
+#endif /* __CROS_EC_USB_PD_CONFIG_H */
+
diff --git a/board/servo_v4/usb_pd_policy.c b/board/servo_v4/usb_pd_policy.c
new file mode 100644
index 0000000000..e26562794b
--- /dev/null
+++ b/board/servo_v4/usb_pd_policy.c
@@ -0,0 +1,184 @@
+/* Copyright 2017 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 "charge_manager.h"
+#include "common.h"
+#include "console.h"
+#include "gpio.h"
+#include "hooks.h"
+#include "host_command.h"
+#include "i2c.h"
+#include "registers.h"
+#include "system.h"
+#include "task.h"
+#include "timer.h"
+#include "util.h"
+#include "usb_mux.h"
+#include "usb_pd.h"
+
+#define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args)
+#define CPRINTS(format, args...) cprints(CC_USBPD, format, ## args)
+
+/* Define typical operating power and max power */
+/*#define OPERATING_POWER_MW 15000 */
+/*#define MAX_POWER_MW       60000 */
+/*#define MAX_CURRENT_MA     3000 */
+
+#define PDO_FIXED_FLAGS (PDO_FIXED_DUAL_ROLE | PDO_FIXED_DATA_SWAP |\
+			 PDO_FIXED_COMM_CAP)
+
+const uint32_t pd_src_pdo[] = {
+		PDO_FIXED(5000,   900, PDO_FIXED_FLAGS),
+};
+const int pd_src_pdo_cnt = ARRAY_SIZE(pd_src_pdo);
+
+const uint32_t pd_snk_pdo[] = {
+		PDO_FIXED(5000, 500, PDO_FIXED_FLAGS),
+		PDO_BATT(4750, 21000, 15000),
+		PDO_VAR(4750, 21000, 3000),
+};
+const int pd_snk_pdo_cnt = ARRAY_SIZE(pd_snk_pdo);
+
+int pd_is_valid_input_voltage(int mv)
+{
+	/* Any voltage less than the max is allowed */
+	return 1;
+}
+
+void pd_transition_voltage(int idx)
+{
+	/*
+	 * TODO(crosbug.com/p/60794): Most likely this function is a don't care
+	 * for servo_v4 since VBUS provided to the DUT port has just an on/off
+	 * control.  For now leave it as a no-op.
+	 */
+}
+
+int pd_set_power_supply_ready(int port)
+{
+	/*
+	 * TODO(crosbug.com/p/60794): Will likely need to set the GPIOs
+	 * DUT_CHG_EN and HOST_OR_CHG_CTL which control whether DUT port
+	 * provides VBUS from Host or CHG port.
+	 */
+	return EC_SUCCESS; /* we are ready */
+}
+
+void pd_power_supply_reset(int port)
+{
+	/*
+	 * TODO(crosbug.com/p/60794): Will need to set the GPIOs
+	 * DUT_CHG_EN and HOST_OR_CHG_CTL which control whether DUT port
+	 * provides VBUS from Host or CHG port.
+	 */
+}
+
+void pd_set_input_current_limit(int port, uint32_t max_ma,
+				uint32_t supply_voltage)
+{
+	/*
+	 * TODO(crosbug.com/p/60794): Placeholder for now so that can compile
+	 * with USB PD support.
+	 */
+}
+
+void typec_set_input_current_limit(int port, uint32_t max_ma,
+				   uint32_t supply_voltage)
+{
+	/*
+	 * TODO(crosbug.com/p/60794): Placeholder for now so that can compile
+	 * with USB PD support.
+	 */
+}
+
+int pd_snk_is_vbus_provided(int port)
+{
+
+	return gpio_get_level(port ? GPIO_USB_DET_PP_DUT :
+				     GPIO_USB_DET_PP_CHG);
+}
+
+int pd_board_checks(void)
+{
+	return EC_SUCCESS;
+}
+
+int pd_check_power_swap(int port)
+{
+	/*
+	 * TODO(crosbug.com/p/60792): CHG port can't do a power swap as it's SNK
+	 * only. DUT port should be able to support a power role swap, but VBUS
+	 * will need to be present. For now, don't allow swaps on either port.
+	 */
+	return 0;
+}
+
+int pd_check_data_swap(int port, int data_role)
+{
+	/* Servo can allow data role swaps */
+	return 1;
+}
+
+void pd_execute_data_swap(int port, int data_role)
+{
+	/* Should we do something here? */
+}
+
+void pd_check_pr_role(int port, int pr_role, int flags)
+{
+	/*
+	 * TODO(crosbug.com/p/60792): CHG port can't do a power swap as it's SNK
+	 * only. DUT port should be able to support a power role swap, but VBUS
+	 * will need to be present. For now, don't allow swaps on either port.
+	 */
+
+}
+
+void pd_check_dr_role(int port, int dr_role, int flags)
+{
+	/*
+	 * TODO(crosbug.com/p/60792): CHG port is SNK only and should not need
+	 * to change from default UFP role. DUT port behavior needs to be
+	 * flushed out. Don't request any data role change for either port for
+	 * now.
+	 */
+}
+
+
+/* ----------------- Vendor Defined Messages ------------------ */
+const struct svdm_response svdm_rsp = {
+	.identity = NULL,
+	.svids = NULL,
+	.modes = NULL,
+};
+
+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;
+}
+
+
+
+const struct svdm_amode_fx supported_modes[] = {};
+const int supported_modes_cnt = ARRAY_SIZE(supported_modes);