servo_v4p1: Add support for alternate charger port

A TypeC charger can be attached to the alternate charger
port to supply up to 15W to the servoV4.1.

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

Signed-off-by: Sam Hurst <shurst@google.com>
Change-Id: Ia88c783d23d09bfc12b34d416af98445db2d75c2
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2270696
Reviewed-by: Brian Nemec <bnemec@chromium.org>
(cherry picked from commit 31146a9a0acdf42ef5148a473242f77886fcbd2e)
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2332250

7 files changed, 955 insertions, 3 deletions

diff --git a/board/servo_v4p1/board.c b/board/servo_v4p1/board.c
index 6ffd05fbe3..6148358aff 100644
--- a/board/servo_v4p1/board.c
+++ b/board/servo_v4p1/board.c
@@ -12,6 +12,7 @@
 #include "console.h"
 #include "dacs.h"
 #include "ec_version.h"
+#include "fusb302b.h"
 #include "gpio.h"
 #include "hooks.h"
 #include "i2c.h"
@@ -145,7 +146,7 @@ static void dp_evt(enum gpio_signal signal)
 
 static void tcpc_evt(enum gpio_signal signal)
 {
-	ccprintf("tcpc event\n");
+	update_status_fusb302b();
 }
 
 static void hub_evt(enum gpio_signal signal)
@@ -180,6 +181,11 @@ void ext_hpd_detection_enable(int enable)
 	}
 }
 #else
+void snk_task(void *u)
+{
+	/* DO NOTHING */
+}
+
 void pd_task(void *u)
 {
 	/* DO NOTHING */
@@ -400,6 +406,7 @@ static void board_init(void)
 	init_uservo_port();
 	init_pathsel();
 	init_ina231s();
+	init_fusb302b(1);
 
 	/* Enable DUT USB2.0 pair. */
 	gpio_set_level(GPIO_FASTBOOT_DUTHUB_MUX_EN_L, 0);
@@ -410,7 +417,6 @@ static void board_init(void)
 
 	gpio_enable_interrupt(GPIO_STM_FAULT_IRQ_L);
 	gpio_enable_interrupt(GPIO_DP_HPD);
-	gpio_enable_interrupt(GPIO_CHGSRV_TCPC_INT_ODL);
 	gpio_enable_interrupt(GPIO_USBH_I2C_BUSY_INT);
 	gpio_enable_interrupt(GPIO_BC12_INT_ODL);
 
diff --git a/board/servo_v4p1/build.mk b/board/servo_v4p1/build.mk
index 3df5847770..a8d22ccbc3 100644
--- a/board/servo_v4p1/build.mk
+++ b/board/servo_v4p1/build.mk
@@ -26,5 +26,8 @@ board-ro+=pathsel.o
 board-ro+=chg_control.o
 board-ro+=ina231s.o
 board-ro+=usb_pd_policy.o
+board-ro+=fusb302b.o
+board-ro+=usb_sm.o
+board-ro+=usb_tc_snk_sm.o
 
 all_deps=$(patsubst ro,,$(def_all_deps))
diff --git a/board/servo_v4p1/ec.tasklist b/board/servo_v4p1/ec.tasklist
index 700315ec90..51bed39b63 100644
--- a/board/servo_v4p1/ec.tasklist
+++ b/board/servo_v4p1/ec.tasklist
@@ -10,4 +10,5 @@
 	TASK_ALWAYS(HOOKS,   hook_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)
+	TASK_ALWAYS(PD_C1,   pd_task,      NULL, VENTI_TASK_STACK_SIZE) \
+	TASK_ALWAYS(PD_C2,   snk_task,     NULL, VENTI_TASK_STACK_SIZE)
diff --git a/board/servo_v4p1/fusb302b.c b/board/servo_v4p1/fusb302b.c
new file mode 100644
index 0000000000..fabb5b80ad
--- /dev/null
+++ b/board/servo_v4p1/fusb302b.c
@@ -0,0 +1,219 @@
+/* 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 "i2c.h"
+#include "fusb302b.h"
+#include "gpio.h"
+#include "hooks.h"
+#include "ioexpanders.h"
+#include "util.h"
+#include "task.h"
+#include "time.h"
+#include "usb_pd.h"
+
+static int port;
+static int status0;
+static int status1;
+static int interrupt;
+static struct mutex measure_lock;
+
+static int tcpc_write(int reg, int val)
+{
+	return i2c_write8(port, FUSB302_I2C_SLAVE_ADDR_FLAGS, reg, val);
+}
+
+static int tcpc_read(int reg, int *val)
+{
+	return i2c_read8(port, FUSB302_I2C_SLAVE_ADDR_FLAGS, reg, val);
+}
+
+int init_fusb302b(int p)
+{
+	int ret;
+	int reg;
+	int interrupta;
+	int interruptb;
+
+	/* Configure fusb302b for SNK only operation */
+	port = p;
+
+	ret = tcpc_write(TCPC_REG_RESET, TCPC_REG_RESET_SW_RESET);
+	if (ret)
+		return ret;
+
+	/* Create interrupt masks */
+	reg = 0xFF;
+	/* CC level changes */
+	reg &= ~TCPC_REG_MASK_BC_LVL;
+	/* misc alert */
+	reg &= ~TCPC_REG_MASK_ALERT;
+	/* VBUS threshold crossed (~4.0V) */
+	reg &= ~TCPC_REG_MASK_VBUSOK;
+	tcpc_write(TCPC_REG_MASK, reg);
+
+	/* Interrupt Enable */
+	ret = tcpc_read(TCPC_REG_CONTROL0, &reg);
+	if (ret)
+		return ret;
+
+	reg &= ~TCPC_REG_CONTROL0_INT_MASK;
+	ret = tcpc_write(TCPC_REG_CONTROL0, reg);
+	if (ret)
+		return ret;
+
+	ret = tcpc_write(TCPC_REG_POWER, TCPC_REG_POWER_PWR_ALL);
+	if (ret)
+		return ret;
+
+	/* reading interrupt registers clears them */
+	ret = tcpc_read(TCPC_REG_INTERRUPT, &interrupt);
+	if (ret)
+		return ret;
+
+
+	ret = tcpc_read(TCPC_REG_INTERRUPTA, &interrupta);
+	if (ret)
+		return ret;
+
+	ret = tcpc_read(TCPC_REG_INTERRUPTB, &interruptb);
+	if (ret)
+		return ret;
+
+	/* Call this, will detect a charger that's already plugged in */
+	update_status_fusb302b();
+
+	/* Enable interrupt */
+	gpio_enable_interrupt(GPIO_CHGSRV_TCPC_INT_ODL);
+
+	return EC_SUCCESS;
+}
+
+void fusb302b_irq(void)
+{
+	tcpc_read(TCPC_REG_INTERRUPT, &interrupt);
+	tcpc_read(TCPC_REG_STATUS0, &status0);
+	tcpc_read(TCPC_REG_STATUS1, &status1);
+
+	task_wake(TASK_ID_PD_C2);
+}
+DECLARE_DEFERRED(fusb302b_irq);
+
+int update_status_fusb302b(void)
+{
+	hook_call_deferred(&fusb302b_irq_data, 0);
+	return EC_SUCCESS;
+}
+
+int is_vbus_present(void)
+{
+	return (status0 & 0x80);
+}
+
+/* Convert BC LVL values (in FUSB302) to Type-C CC Voltage Status */
+static int convert_bc_lvl(int bc_lvl)
+{
+	int ret;
+
+	switch (bc_lvl) {
+	case 1:
+		ret = TYPEC_CC_VOLT_RP_DEF;
+		break;
+	case 2:
+		ret = TYPEC_CC_VOLT_RP_1_5;
+		break;
+	case 3:
+		ret = TYPEC_CC_VOLT_RP_3_0;
+		break;
+	default:
+		ret = TYPEC_CC_VOLT_OPEN;
+	}
+
+	return ret;
+}
+
+int get_cc(int *cc1, int *cc2)
+{
+	int reg;
+	int orig_meas_cc1;
+	int orig_meas_cc2;
+	int bc_lvl_cc1;
+	int bc_lvl_cc2;
+
+	mutex_lock(&measure_lock);
+
+	/*
+	 * Measure CC1 first.
+	 */
+	tcpc_read(TCPC_REG_SWITCHES0, &reg);
+
+	/* save original state to be returned to later... */
+	if (reg & TCPC_REG_SWITCHES0_MEAS_CC1)
+		orig_meas_cc1 = 1;
+	else
+		orig_meas_cc1 = 0;
+
+	if (reg & TCPC_REG_SWITCHES0_MEAS_CC2)
+		orig_meas_cc2 = 1;
+	else
+		orig_meas_cc2 = 0;
+
+
+	/* Disable CC2 measurement switch, enable CC1 measurement switch */
+	reg &= ~TCPC_REG_SWITCHES0_MEAS_CC2;
+	reg |= TCPC_REG_SWITCHES0_MEAS_CC1;
+
+	tcpc_write(TCPC_REG_SWITCHES0, reg);
+
+	/* CC1 is now being measured by FUSB302. */
+
+	/* Wait on measurement */
+	usleep(250);
+
+	tcpc_read(TCPC_REG_STATUS0, &bc_lvl_cc1);
+
+	/* mask away unwanted bits */
+	bc_lvl_cc1 &= (TCPC_REG_STATUS0_BC_LVL0 | TCPC_REG_STATUS0_BC_LVL1);
+
+	/*
+	 * Measure CC2 next.
+	 */
+
+	tcpc_read(TCPC_REG_SWITCHES0, &reg);
+
+	/* Disable CC1 measurement switch, enable CC2 measurement switch */
+	reg &= ~TCPC_REG_SWITCHES0_MEAS_CC1;
+	reg |= TCPC_REG_SWITCHES0_MEAS_CC2;
+
+	tcpc_write(TCPC_REG_SWITCHES0, reg);
+
+	/* CC2 is now being measured by FUSB302. */
+
+	/* Wait on measurement */
+	usleep(250);
+
+	tcpc_read(TCPC_REG_STATUS0, &bc_lvl_cc2);
+
+	/* mask away unwanted bits */
+	bc_lvl_cc2 &= (TCPC_REG_STATUS0_BC_LVL0 | TCPC_REG_STATUS0_BC_LVL1);
+
+	*cc1 = convert_bc_lvl(bc_lvl_cc1);
+	*cc2 = convert_bc_lvl(bc_lvl_cc2);
+
+	/* return MEAS_CC1/2 switches to original state */
+	tcpc_read(TCPC_REG_SWITCHES0, &reg);
+	if (orig_meas_cc1)
+		reg |= TCPC_REG_SWITCHES0_MEAS_CC1;
+	else
+		reg &= ~TCPC_REG_SWITCHES0_MEAS_CC1;
+	if (orig_meas_cc2)
+		reg |= TCPC_REG_SWITCHES0_MEAS_CC2;
+	else
+		reg &= ~TCPC_REG_SWITCHES0_MEAS_CC2;
+
+	tcpc_write(TCPC_REG_SWITCHES0, reg);
+
+	mutex_unlock(&measure_lock);
+	return 0;
+}
diff --git a/board/servo_v4p1/fusb302b.h b/board/servo_v4p1/fusb302b.h
new file mode 100644
index 0000000000..2f0cd207b8
--- /dev/null
+++ b/board/servo_v4p1/fusb302b.h
@@ -0,0 +1,239 @@
+/* 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.
+ */
+
+/* USB Power delivery port management */
+/* For Fairchild FUSB302 */
+#ifndef __CROS_EC_DRIVER_TCPM_FUSB302_H
+#define __CROS_EC_DRIVER_TCPM_FUSB302_H
+
+/* Chip Device ID - 302A or 302B */
+#define FUSB302_DEVID_302A 0x08
+#define FUSB302_DEVID_302B 0x09
+
+/* I2C slave address varies by part number */
+/* FUSB302BUCX / FUSB302BMPX */
+#define FUSB302_I2C_SLAVE_ADDR_FLAGS 0x22
+/* FUSB302B01MPX */
+#define FUSB302_I2C_SLAVE_ADDR_B01_FLAGS 0x23
+/* FUSB302B10MPX */
+#define FUSB302_I2C_SLAVE_ADDR_B10_FLAGS 0x24
+/* FUSB302B11MPX */
+#define FUSB302_I2C_SLAVE_ADDR_B11_FLAGS 0x25
+
+/* Default retry count for transmitting */
+#define PD_RETRY_COUNT		3
+
+#define TCPC_REG_DEVICE_ID	0x01
+
+#define TCPC_REG_SWITCHES0	0x02
+#define TCPC_REG_SWITCHES0_CC2_PU_EN	(1<<7)
+#define TCPC_REG_SWITCHES0_CC1_PU_EN	(1<<6)
+#define TCPC_REG_SWITCHES0_VCONN_CC2	(1<<5)
+#define TCPC_REG_SWITCHES0_VCONN_CC1	(1<<4)
+#define TCPC_REG_SWITCHES0_MEAS_CC2	(1<<3)
+#define TCPC_REG_SWITCHES0_MEAS_CC1	(1<<2)
+#define TCPC_REG_SWITCHES0_CC2_PD_EN	(1<<1)
+#define TCPC_REG_SWITCHES0_CC1_PD_EN	(1<<0)
+
+#define TCPC_REG_SWITCHES1	0x03
+#define TCPC_REG_SWITCHES1_POWERROLE	(1<<7)
+#define TCPC_REG_SWITCHES1_SPECREV1	(1<<6)
+#define TCPC_REG_SWITCHES1_SPECREV0	(1<<5)
+#define TCPC_REG_SWITCHES1_DATAROLE	(1<<4)
+#define TCPC_REG_SWITCHES1_AUTO_GCRC	(1<<2)
+#define TCPC_REG_SWITCHES1_TXCC2_EN	(1<<1)
+#define TCPC_REG_SWITCHES1_TXCC1_EN	(1<<0)
+
+#define TCPC_REG_MEASURE	0x04
+#define TCPC_REG_MEASURE_MDAC_MASK	0x3F
+#define TCPC_REG_MEASURE_VBUS		(1<<6)
+/*
+ * MDAC reference voltage step size is 42 mV. Round our thresholds to reduce
+ * maximum error, which also matches suggested thresholds in datasheet
+ * (Table 3. Host Interrupt Summary).
+ */
+#define TCPC_REG_MEASURE_MDAC_MV(mv)	(DIV_ROUND_NEAREST((mv), 42) & 0x3f)
+
+#define TCPC_REG_CONTROL0	0x06
+#define TCPC_REG_CONTROL0_TX_FLUSH	(1<<6)
+#define TCPC_REG_CONTROL0_INT_MASK	(1<<5)
+#define TCPC_REG_CONTROL0_HOST_CUR_MASK (3<<2)
+#define TCPC_REG_CONTROL0_HOST_CUR_3A0  (3<<2)
+#define TCPC_REG_CONTROL0_HOST_CUR_1A5  (2<<2)
+#define TCPC_REG_CONTROL0_HOST_CUR_USB  (1<<2)
+#define TCPC_REG_CONTROL0_TX_START	(1<<0)
+
+#define TCPC_REG_CONTROL1	0x07
+#define TCPC_REG_CONTROL1_ENSOP2DB	(1<<6)
+#define TCPC_REG_CONTROL1_ENSOP1DB	(1<<5)
+#define TCPC_REG_CONTROL1_BIST_MODE2	(1<<4)
+#define TCPC_REG_CONTROL1_RX_FLUSH	(1<<2)
+#define TCPC_REG_CONTROL1_ENSOP2	(1<<1)
+#define TCPC_REG_CONTROL1_ENSOP1	(1<<0)
+
+#define TCPC_REG_CONTROL2	0x08
+/* two-bit field, valid values below */
+#define TCPC_REG_CONTROL2_MODE_MASK	(0x3<<TCPC_REG_CONTROL2_MODE_POS)
+#define TCPC_REG_CONTROL2_MODE_DFP	(0x3)
+#define TCPC_REG_CONTROL2_MODE_UFP	(0x2)
+#define TCPC_REG_CONTROL2_MODE_DRP	(0x1)
+#define TCPC_REG_CONTROL2_MODE_POS	(1)
+#define TCPC_REG_CONTROL2_TOGGLE	(1<<0)
+
+#define TCPC_REG_CONTROL3	0x09
+#define TCPC_REG_CONTROL3_SEND_HARDRESET	(1<<6)
+#define TCPC_REG_CONTROL3_BIST_TMODE		(1<<5) /* 302B Only */
+#define TCPC_REG_CONTROL3_AUTO_HARDRESET	(1<<4)
+#define TCPC_REG_CONTROL3_AUTO_SOFTRESET	(1<<3)
+/* two-bit field */
+#define TCPC_REG_CONTROL3_N_RETRIES		(1<<1)
+#define TCPC_REG_CONTROL3_N_RETRIES_POS		(1)
+#define TCPC_REG_CONTROL3_N_RETRIES_SIZE	(2)
+#define TCPC_REG_CONTROL3_AUTO_RETRY		(1<<0)
+
+#define TCPC_REG_MASK		0x0A
+#define TCPC_REG_MASK_VBUSOK		(1<<7)
+#define TCPC_REG_MASK_ACTIVITY		(1<<6)
+#define TCPC_REG_MASK_COMP_CHNG		(1<<5)
+#define TCPC_REG_MASK_CRC_CHK		(1<<4)
+#define TCPC_REG_MASK_ALERT		(1<<3)
+#define TCPC_REG_MASK_WAKE		(1<<2)
+#define TCPC_REG_MASK_COLLISION		(1<<1)
+#define TCPC_REG_MASK_BC_LVL		(1<<0)
+
+#define TCPC_REG_POWER		0x0B
+#define TCPC_REG_POWER_PWR		(1<<0)	/* four-bit field */
+#define TCPC_REG_POWER_PWR_LOW		0x1 /* Bandgap + Wake circuitry */
+#define TCPC_REG_POWER_PWR_MEDIUM	0x3 /* LOW + Receiver + Current refs */
+#define TCPC_REG_POWER_PWR_HIGH		0x7 /* MEDIUM + Measure block */
+#define TCPC_REG_POWER_PWR_ALL		0xF /* HIGH + Internal Oscillator */
+
+#define TCPC_REG_RESET		0x0C
+#define TCPC_REG_RESET_PD_RESET		(1<<1)
+#define TCPC_REG_RESET_SW_RESET		(1<<0)
+
+#define TCPC_REG_MASKA		0x0E
+#define TCPC_REG_MASKA_OCP_TEMP		(1<<7)
+#define TCPC_REG_MASKA_TOGDONE		(1<<6)
+#define TCPC_REG_MASKA_SOFTFAIL		(1<<5)
+#define TCPC_REG_MASKA_RETRYFAIL	(1<<4)
+#define TCPC_REG_MASKA_HARDSENT		(1<<3)
+#define TCPC_REG_MASKA_TX_SUCCESS	(1<<2)
+#define TCPC_REG_MASKA_SOFTRESET	(1<<1)
+#define TCPC_REG_MASKA_HARDRESET	(1<<0)
+
+#define TCPC_REG_MASKB		0x0F
+#define TCPC_REG_MASKB_GCRCSENT		(1<<0)
+
+#define TCPC_REG_STATUS0A	0x3C
+#define TCPC_REG_STATUS0A_SOFTFAIL	(1<<5)
+#define TCPC_REG_STATUS0A_RETRYFAIL	(1<<4)
+#define TCPC_REG_STATUS0A_POWER		(1<<2) /* two-bit field */
+#define TCPC_REG_STATUS0A_RX_SOFT_RESET	(1<<1)
+#define TCPC_REG_STATUS0A_RX_HARD_RESEt	(1<<0)
+
+#define TCPC_REG_STATUS1A	0x3D
+/* three-bit field, valid values below */
+#define TCPC_REG_STATUS1A_TOGSS		(1<<3)
+#define TCPC_REG_STATUS1A_TOGSS_RUNNING		0x0
+#define TCPC_REG_STATUS1A_TOGSS_SRC1		0x1
+#define TCPC_REG_STATUS1A_TOGSS_SRC2		0x2
+#define TCPC_REG_STATUS1A_TOGSS_SNK1		0x5
+#define TCPC_REG_STATUS1A_TOGSS_SNK2		0x6
+#define TCPC_REG_STATUS1A_TOGSS_AA		0x7
+#define TCPC_REG_STATUS1A_TOGSS_POS		(3)
+#define TCPC_REG_STATUS1A_TOGSS_MASK		(0x7)
+
+#define TCPC_REG_STATUS1A_RXSOP2DB	(1<<2)
+#define TCPC_REG_STATUS1A_RXSOP1DB	(1<<1)
+#define TCPC_REG_STATUS1A_RXSOP		(1<<0)
+
+#define TCPC_REG_INTERRUPTA	0x3E
+#define TCPC_REG_INTERRUPTA_OCP_TEMP	(1<<7)
+#define TCPC_REG_INTERRUPTA_TOGDONE	(1<<6)
+#define TCPC_REG_INTERRUPTA_SOFTFAIL	(1<<5)
+#define TCPC_REG_INTERRUPTA_RETRYFAIL	(1<<4)
+#define TCPC_REG_INTERRUPTA_HARDSENT	(1<<3)
+#define TCPC_REG_INTERRUPTA_TX_SUCCESS	(1<<2)
+#define TCPC_REG_INTERRUPTA_SOFTRESET	(1<<1)
+#define TCPC_REG_INTERRUPTA_HARDRESET	(1<<0)
+
+#define TCPC_REG_INTERRUPTB	0x3F
+#define TCPC_REG_INTERRUPTB_GCRCSENT		(1<<0)
+
+#define TCPC_REG_STATUS0	0x40
+#define TCPC_REG_STATUS0_VBUSOK		(1<<7)
+#define TCPC_REG_STATUS0_ACTIVITY	(1<<6)
+#define TCPC_REG_STATUS0_COMP		(1<<5)
+#define TCPC_REG_STATUS0_CRC_CHK	(1<<4)
+#define TCPC_REG_STATUS0_ALERT		(1<<3)
+#define TCPC_REG_STATUS0_WAKE		(1<<2)
+#define TCPC_REG_STATUS0_BC_LVL1	(1<<1) /* two-bit field */
+#define TCPC_REG_STATUS0_BC_LVL0	(1<<0) /* two-bit field */
+
+#define TCPC_REG_STATUS1	0x41
+#define TCPC_REG_STATUS1_RXSOP2		(1<<7)
+#define TCPC_REG_STATUS1_RXSOP1		(1<<6)
+#define TCPC_REG_STATUS1_RX_EMPTY	(1<<5)
+#define TCPC_REG_STATUS1_RX_FULL	(1<<4)
+#define TCPC_REG_STATUS1_TX_EMPTY	(1<<3)
+#define TCPC_REG_STATUS1_TX_FULL	(1<<2)
+
+#define TCPC_REG_INTERRUPT	0x42
+#define TCPC_REG_INTERRUPT_VBUSOK	(1<<7)
+#define TCPC_REG_INTERRUPT_ACTIVITY	(1<<6)
+#define TCPC_REG_INTERRUPT_COMP_CHNG	(1<<5)
+#define TCPC_REG_INTERRUPT_CRC_CHK	(1<<4)
+#define TCPC_REG_INTERRUPT_ALERT	(1<<3)
+#define TCPC_REG_INTERRUPT_WAKE		(1<<2)
+#define TCPC_REG_INTERRUPT_COLLISION	(1<<1)
+#define TCPC_REG_INTERRUPT_BC_LVL	(1<<0)
+
+#define TCPC_REG_FIFOS		0x43
+
+/* Tokens defined for the FUSB302 TX FIFO */
+enum fusb302_txfifo_tokens {
+	FUSB302_TKN_TXON = 0xA1,
+	FUSB302_TKN_SYNC1 = 0x12,
+	FUSB302_TKN_SYNC2 = 0x13,
+	FUSB302_TKN_SYNC3 = 0x1B,
+	FUSB302_TKN_RST1 = 0x15,
+	FUSB302_TKN_RST2 = 0x16,
+	FUSB302_TKN_PACKSYM = 0x80,
+	FUSB302_TKN_JAMCRC = 0xFF,
+	FUSB302_TKN_EOP = 0x14,
+	FUSB302_TKN_TXOFF = 0xFE,
+};
+
+/**
+ * Initializes the FUSB302 to operate
+ * as a SNK only.
+ *
+ * @param port  The i2c bus of the FUSB302B
+ *
+ * @returns EC_SUCCESS or EC_XXX on error
+ */
+int init_fusb302b(int port);
+
+/**
+ * Should be called from the interrupt generated
+ * by the FUSB302. This function reads status
+ * and interrupt registers in the FUSB302.
+ */
+int update_status_fusb302b(void);
+
+/**
+ * Returns true if VBUS is present, else false
+ */
+int is_vbus_present(void);
+
+/*
+ * Reads the status of the CC lines
+ *
+ * @returns EC_SUCCESS or EC_XXX on failure
+ */
+int get_cc(int *cc1, int *cc2);
+
+#endif /* __CROS_EC_DRIVER_TCPM_FUSB302_H */
diff --git a/board/servo_v4p1/usb_sm.c b/board/servo_v4p1/usb_sm.c
new file mode 100644
index 0000000000..94b5e0c08d
--- /dev/null
+++ b/board/servo_v4p1/usb_sm.c
@@ -0,0 +1,193 @@
+/* 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 "common.h"
+#include "console.h"
+#include "stdbool.h"
+#include "usb_pd.h"
+#include "usb_sm.h"
+#include "util.h"
+
+#ifdef CONFIG_COMMON_RUNTIME
+#define CPRINTF(format, args...) cprintf(CC_USB, format, ## args)
+#define CPRINTS(format, args...) cprints(CC_USB, format, ## args)
+#else /* CONFIG_COMMON_RUNTIME */
+#define CPRINTF(format, args...)
+#define CPRINTS(format, args...)
+#endif
+
+/* Private structure (to this file) used to track state machine context */
+struct internal_ctx {
+	usb_state_ptr last_entered;
+	uint32_t running : 1;
+	uint32_t enter	 : 1;
+	uint32_t exit	 : 1;
+};
+BUILD_ASSERT(sizeof(struct internal_ctx) ==
+	     member_size(struct sm_ctx, internal));
+
+/* Gets the first shared parent state between a and b (inclusive) */
+static usb_state_ptr shared_parent_state(usb_state_ptr a, usb_state_ptr b)
+{
+	const usb_state_ptr orig_b = b;
+
+	/* There are no common ancestors */
+	if (b == NULL)
+		return NULL;
+
+	/* This assumes that both A and B are NULL terminated without cycles */
+	while (a != NULL) {
+		/* We found a match return */
+		if (a == b)
+			return a;
+
+		/*
+		 * Otherwise, increment b down the list for comparison until we
+		 * run out, then increment a and start over on b for comparison
+		 */
+		if (b->parent == NULL) {
+			a = a->parent;
+			b = orig_b;
+		} else {
+			b = b->parent;
+		}
+	}
+
+	return NULL;
+}
+
+/*
+ * Call all entry functions of parents before children. If set_state is called
+ * during one of the entry functions, then do not call any remaining entry
+ * functions.
+ */
+static void call_entry_functions(const int port,
+			       struct internal_ctx *const internal,
+			       const usb_state_ptr stop,
+			       const usb_state_ptr current)
+{
+	if (current == stop)
+		return;
+
+	call_entry_functions(port, internal, stop, current->parent);
+
+	/*
+	 * If the previous entry function called set_state, then don't enter
+	 * remaining states.
+	 */
+	if (!internal->enter)
+		return;
+
+	/* Track the latest state that was entered, so we can exit properly. */
+	internal->last_entered = current;
+	if (current->entry)
+		current->entry(port);
+}
+
+/*
+ * Call all exit functions of children before parents. Note set_state is ignored
+ * during an exit function.
+ */
+static void call_exit_functions(const int port, const usb_state_ptr stop,
+			      const usb_state_ptr current)
+{
+	if (current == stop)
+		return;
+
+	if (current->exit)
+		current->exit(port);
+
+	call_exit_functions(port, stop, current->parent);
+}
+
+void set_state(const int port, struct sm_ctx *const ctx,
+	       const usb_state_ptr new_state)
+{
+	struct internal_ctx * const internal = (void *) ctx->internal;
+	usb_state_ptr last_state;
+	usb_state_ptr shared_parent;
+
+	/*
+	 * It does not make sense to call set_state in an exit phase of a state
+	 * since we are already in a transition; we would always ignore the
+	 * intended state to transition into.
+	 */
+	if (internal->exit) {
+		CPRINTF("C%d: Ignoring set state to 0x%pP within 0x%pP",
+			port, new_state, ctx->current);
+		return;
+	}
+
+	/*
+	 * Determine the last state that was entered. Normally it is current,
+	 * but we could have called set_state within an entry phase, so we
+	 * shouldn't exit any states that weren't fully entered.
+	 */
+	last_state = internal->enter ? internal->last_entered : ctx->current;
+
+	/* We don't exit and re-enter shared parent states */
+	shared_parent = shared_parent_state(last_state, new_state);
+
+	/*
+	 * Exit all of the non-common states from the last state.
+	 */
+	internal->exit = true;
+	call_exit_functions(port, shared_parent, last_state);
+	internal->exit = false;
+
+	ctx->previous = ctx->current;
+	ctx->current = new_state;
+
+	/*
+	 * Enter all new non-common states. last_entered will contain the last
+	 * state that successfully entered before another set_state was called.
+	 */
+	internal->last_entered = NULL;
+	internal->enter = true;
+	call_entry_functions(port, internal, shared_parent, ctx->current);
+	/*
+	 * Setting enter to false ensures that all pending entry calls will be
+	 * skipped (in the case of a parent state calling set_state, which means
+	 * we should not enter any child states)
+	 */
+	internal->enter = false;
+
+	/*
+	 * If we set_state while we are running a child state, then stop running
+	 * any remaining parent states.
+	 */
+	internal->running = false;
+}
+
+/*
+ * Call all run functions of children before parents. If set_state is called
+ * during one of the entry functions, then do not call any remaining entry
+ * functions.
+ */
+static void call_run_functions(const int port,
+			     const struct internal_ctx *const internal,
+			     const usb_state_ptr current)
+{
+	if (!current)
+		return;
+
+	/* If set_state is called during run, don't call remain functions. */
+	if (!internal->running)
+		return;
+
+	if (current->run)
+		current->run(port);
+
+	call_run_functions(port, internal, current->parent);
+}
+
+void run_state(const int port, struct sm_ctx *const ctx)
+{
+	struct internal_ctx * const internal = (void *) ctx->internal;
+
+	internal->running = true;
+	call_run_functions(port, internal, ctx->current);
+	internal->running = false;
+}
diff --git a/board/servo_v4p1/usb_tc_snk_sm.c b/board/servo_v4p1/usb_tc_snk_sm.c
new file mode 100644
index 0000000000..f9a3966434
--- /dev/null
+++ b/board/servo_v4p1/usb_tc_snk_sm.c
@@ -0,0 +1,291 @@
+/* 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 "common.h"
+#include "console.h"
+#include "fusb302b.h"
+#include "ioexpanders.h"
+#include "system.h"
+#include "task.h"
+#include "usb_common.h"
+#include "usb_pd.h"
+#include "usb_sm.h"
+#include "usb_tc_sm.h"
+
+#define EVT_TIMEOUT_NEVER  (-1)
+#define EVT_TIMEOUT_5MS    (5 * MSEC)
+
+/*
+ * USB Type-C Sink
+ *   See Figure 4-13 in Release 1.4 of USB Type-C Spec.
+ */
+#define CPRINTF(format, args...) cprintf(CC_USBPD, format, ## args)
+#define CPRINTS(format, args...) cprints(CC_USBPD, format, ## args)
+
+/* Type-C Layer Flags */
+
+/* List of all TypeC-level states */
+enum usb_tc_state {
+	TC_UNATTACHED_SNK,
+	TC_ATTACH_WAIT_SNK,
+	TC_ATTACHED_SNK,
+};
+/* Forward declare the full list of states. This is indexed by usb_tc_state */
+static const struct usb_state tc_states[];
+
+/* TypeC Power strings */
+static const char * const pwr2_5_str = "5V/0.5A";
+static const char * const pwr7_5_str = "5V/1.5A";
+static const char * const pwr15_str  = "5V/3A";
+
+static struct type_c {
+	/* state machine context */
+	struct sm_ctx ctx;
+	/* Port polarity */
+	enum tcpc_cc_polarity polarity;
+	/* event timeout */
+	uint64_t evt_timeout;
+	/* Time a port shall wait before it can determine it is attached */
+	uint64_t cc_debounce;
+	/*
+	 * Time a Sink port shall wait before it can determine it is detached
+	 * due to the potential for USB PD signaling on CC as described in
+	 * the state definitions.
+	 */
+	uint64_t pd_debounce;
+	/* The cc state */
+	enum pd_cc_states cc_state;
+	/* Generic timer */
+	uint64_t timeout;
+	/* Voltage on CC pin */
+	enum tcpc_cc_voltage_status cc_voltage;
+	/* Current CC1 value */
+	int cc1;
+	/* Current CC2 value */
+	int cc2;
+} tc;
+
+/* Forward declare common, private functions */
+static void set_state_tc(const enum usb_tc_state new_state);
+
+static void restart_tc_sm(enum usb_tc_state start_state)
+{
+	int res;
+
+	res = init_fusb302b(1);
+	CPRINTS("FUSB302b init %s", res ? "failed" : "ready");
+
+	/* State machine is disabled if init_fusb302b fails */
+	if (!res)
+		set_state_tc(start_state);
+
+	/* Disable timeout. Task will wake on interrupt */
+	tc.evt_timeout = EVT_TIMEOUT_NEVER;
+}
+
+/*
+ * Private Functions
+ */
+
+/* Set the TypeC state machine to a new state. */
+static void set_state_tc(const enum usb_tc_state new_state)
+{
+	set_state(0, &tc.ctx, &tc_states[new_state]);
+}
+
+static void print_alt_power(void)
+{
+	enum tcpc_cc_voltage_status cc;
+	char const *pwr;
+
+	cc = tc.polarity ? tc.cc2 : tc.cc1;
+	if (cc == TYPEC_CC_VOLT_OPEN ||
+		cc == TYPEC_CC_VOLT_RA || cc == TYPEC_CC_VOLT_RD) {
+		/* Supply removed or not detected */
+		return;
+	}
+
+	if (cc == TYPEC_CC_VOLT_RP_1_5)
+		pwr = pwr7_5_str;
+	else if (cc == TYPEC_CC_VOLT_RP_3_0)
+		pwr = pwr15_str;
+	else
+		pwr = pwr2_5_str;
+
+	CPRINTS("ALT: Switching to alternate supply @ %s", pwr);
+}
+
+static void sink_power_sub_states(void)
+{
+	enum tcpc_cc_voltage_status cc;
+	enum tcpc_cc_voltage_status new_cc_voltage;
+
+	cc = tc.polarity ? tc.cc2 : tc.cc1;
+
+	if (cc == TYPEC_CC_VOLT_RP_DEF)
+		new_cc_voltage = TYPEC_CC_VOLT_RP_DEF;
+	else if (cc == TYPEC_CC_VOLT_RP_1_5)
+		new_cc_voltage = TYPEC_CC_VOLT_RP_1_5;
+	else if (cc == TYPEC_CC_VOLT_RP_3_0)
+		new_cc_voltage = TYPEC_CC_VOLT_RP_3_0;
+	else
+		new_cc_voltage = TYPEC_CC_VOLT_OPEN;
+
+	/* Debounce the cc state */
+	if (new_cc_voltage != tc.cc_voltage) {
+		tc.cc_voltage = new_cc_voltage;
+		tc.cc_debounce = get_time().val + PD_T_RP_VALUE_CHANGE;
+		return;
+	}
+
+	if (tc.cc_debounce == 0 || get_time().val < tc.cc_debounce)
+		return;
+
+	tc.cc_debounce = 0;
+	print_alt_power();
+}
+
+/*
+ * TYPE-C State Implementations
+ */
+
+/**
+ * Unattached.SNK
+ */
+static void tc_unattached_snk_entry(int port)
+{
+	tc.evt_timeout = EVT_TIMEOUT_NEVER;
+}
+
+static void tc_unattached_snk_run(int port)
+{
+	/*
+	 * The port shall transition to AttachWait.SNK when a Source
+	 * connection is detected, as indicated by the SNK.Rp state
+	 * on at least one of its CC pins.
+	 */
+	if (cc_is_rp(tc.cc1) || cc_is_rp(tc.cc2))
+		set_state_tc(TC_ATTACH_WAIT_SNK);
+}
+
+/**
+ * AttachWait.SNK
+ */
+static void tc_attach_wait_snk_entry(int port)
+{
+	tc.evt_timeout = EVT_TIMEOUT_5MS;
+	tc.cc_state = PD_CC_UNSET;
+}
+
+static void tc_attach_wait_snk_run(int port)
+{
+	enum pd_cc_states new_cc_state;
+
+	if (cc_is_rp(tc.cc1) && cc_is_rp(tc.cc2))
+		new_cc_state = PD_CC_DFP_DEBUG_ACC;
+	else if (cc_is_rp(tc.cc1) || cc_is_rp(tc.cc2))
+		new_cc_state = PD_CC_DFP_ATTACHED;
+	else
+		new_cc_state = PD_CC_NONE;
+
+	/* Debounce the cc state */
+	if (new_cc_state != tc.cc_state) {
+		tc.cc_debounce = get_time().val + PD_T_CC_DEBOUNCE;
+		tc.pd_debounce = get_time().val + PD_T_PD_DEBOUNCE;
+		tc.cc_state = new_cc_state;
+		return;
+	}
+
+	/* Wait for CC debounce */
+	if (get_time().val < tc.cc_debounce)
+		return;
+
+	/*
+	 * The port shall transition to Attached.SNK after the state of only
+	 * one of the CC1 or CC2 pins is SNK.Rp for at least tCCDebounce and
+	 * VBUS is detected.
+	 */
+	if (is_vbus_present() && (new_cc_state == PD_CC_DFP_ATTACHED))
+		set_state_tc(TC_ATTACHED_SNK);
+	else
+		set_state_tc(TC_UNATTACHED_SNK);
+}
+
+/**
+ * Attached.SNK
+ */
+static void tc_attached_snk_entry(int port)
+{
+	print_alt_power();
+
+	tc.evt_timeout = EVT_TIMEOUT_NEVER;
+	tc.cc_debounce = 0;
+
+	/* Switch over to alternate supply */
+	en_pp5000_alt_3p3(1);
+}
+
+static void tc_attached_snk_run(int port)
+{
+	/* Detach detection */
+	if (!is_vbus_present()) {
+		set_state_tc(TC_UNATTACHED_SNK);
+		return;
+	}
+
+	/* Run Sink Power Sub-State */
+	sink_power_sub_states();
+}
+
+static void tc_attached_snk_exit(int port)
+{
+	/* Alternate charger removed. Switch back to host power */
+	en_pp5000_alt_3p3(0);
+}
+
+/*
+ * Type-C State
+ *
+ * TC_UNATTACHED_SNK
+ * TC_ATTACH_WAIT_SNK
+ * TC_TRY_WAIT_SNK
+ * TC_ATTACHED_SNK
+ */
+static const struct usb_state tc_states[] = {
+	[TC_UNATTACHED_SNK] = {
+		.entry	= tc_unattached_snk_entry,
+		.run	= tc_unattached_snk_run,
+	},
+	[TC_ATTACH_WAIT_SNK] = {
+		.entry	= tc_attach_wait_snk_entry,
+		.run	= tc_attach_wait_snk_run,
+	},
+	[TC_ATTACHED_SNK] = {
+		.entry	= tc_attached_snk_entry,
+		.run	= tc_attached_snk_run,
+		.exit	= tc_attached_snk_exit,
+	},
+};
+
+void snk_task(void *u)
+{
+	/* Unattached.SNK is the default starting state. */
+	restart_tc_sm(TC_UNATTACHED_SNK);
+
+	while (1) {
+		/* wait for next event or timeout expiration */
+		task_wait_event(tc.evt_timeout);
+
+		/* Sample CC lines */
+		get_cc(&tc.cc1, &tc.cc2);
+
+		/* Detect polarity */
+		tc.polarity = (tc.cc1 > tc.cc2) ? POLARITY_CC1 : POLARITY_CC2;
+
+		/* Run TypeC state machine */
+		run_state(0, &tc.ctx);
+	}
+}
+