First public dropinitdrop

Signed-off-by: Marc Zyngier <maz@kernel.org>

13 files changed, 2718 insertions, 0 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..5d0a2e4
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,35 @@
+# Set minimum required version of CMake
+cmake_minimum_required(VERSION 3.12)
+
+# Include build functions from Pico SDK
+include($ENV{PICO_SDK_PATH}/external/pico_sdk_import.cmake)
+
+# Set name of project (as PROJECT_NAME) and C/C++ standards
+project(m1_ubmc C CXX ASM)
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_CXX_STANDARD 17)
+
+# Creates a pico-sdk subdirectory in our project for the libraries
+pico_sdk_init()
+
+# Tell CMake where to find the executable source file
+add_executable(${PROJECT_NAME} 
+    start.c
+    FUSB302.c
+    tcpm_driver.c
+    vdmtool.c
+)
+
+# Create map/bin/hex/uf2 files
+pico_add_extra_outputs(${PROJECT_NAME})
+
+# Link to pico_stdlib (gpio, time, etc. functions)
+target_link_libraries(${PROJECT_NAME} 
+    pico_stdlib
+    hardware_i2c
+)
+
+# Enable usb output, disable uart output
+pico_enable_stdio_usb(${PROJECT_NAME} 1)
+pico_enable_stdio_uart(${PROJECT_NAME} 0)
+#pico_set_binary_type(${PROJECT_NAME} no_flash)
diff --git a/FUSB302.c b/FUSB302.c
new file mode 100644
index 0000000..6a3e1ab
--- /dev/null
+++ b/FUSB302.c
@@ -0,0 +1,968 @@
+/*  
+    FUSB302.c - Library for interacting with the FUSB302B chip.
+    Copyright 2015 The Chromium OS Authors
+    Copyright 2017 Jason Cerundolo
+    Released under an MIT license. See LICENSE file. 
+*/
+
+#include <stdint.h>
+#include <string.h>
+
+#include "FUSB302.h"
+#include "usb_pd_tcpm.h"
+#include "tcpm_driver.h"
+#include "platform.h"
+
+#define PACKET_IS_GOOD_CRC(head) (PD_HEADER_TYPE(head) == PD_CTRL_GOOD_CRC && \
+				  PD_HEADER_CNT(head) == 0)
+
+static struct fusb302_chip_state {
+	int16_t cc_polarity;
+	int16_t vconn_enabled;
+	/* 1 = pulling up (DFP) 0 = pulling down (UFP) */
+	int16_t pulling_up;
+	int16_t rx_enable;
+	uint8_t control1;
+	uint8_t mdac_vnc;
+	uint8_t mdac_rd;
+	uint8_t msgid;
+} state[CONFIG_USB_PD_PORT_COUNT];
+
+/*
+ * Bring the FUSB302 out of reset after Hard Reset signaling. This will
+ * automatically flush both the Rx and Tx FIFOs.
+ */
+void fusb302_pd_reset(int16_t port)
+{
+	tcpc_write(port, TCPC_REG_RESET, TCPC_REG_RESET_PD_RESET);
+	state[port].msgid = 0;
+}
+
+/*
+ * Flush our Rx FIFO. To prevent packet framing issues, this function should
+ * only be called when Rx is disabled.
+ */
+void fusb302_flush_rx_fifo(int16_t port)
+{
+	tcpc_write(port, TCPC_REG_CONTROL1,
+		   state[port].control1 | TCPC_REG_CONTROL1_RX_FLUSH);
+}
+
+void fusb302_flush_tx_fifo(int16_t port)
+{
+	int16_t reg;
+
+	tcpc_read(port, TCPC_REG_CONTROL0, &reg);
+	reg |= TCPC_REG_CONTROL0_TX_FLUSH;
+	tcpc_write(port, TCPC_REG_CONTROL0, reg);
+}
+
+void fusb302_auto_goodcrc_enable(int16_t port, int16_t enable)
+{
+	int16_t reg;
+
+	tcpc_read(port, TCPC_REG_SWITCHES1, &reg);
+
+	if (enable)
+		reg |= TCPC_REG_SWITCHES1_AUTO_GCRC;
+	else
+		reg &= ~TCPC_REG_SWITCHES1_AUTO_GCRC;
+
+	// Spec says these should be zero, default is bad
+	reg &= ~(TCPC_REG_SWITCHES1_SPECREV0 | TCPC_REG_SWITCHES1_SPECREV1);
+
+	tcpc_write(port, TCPC_REG_SWITCHES1, reg);
+}
+
+/* Convert BC LVL values (in FUSB302) to Type-C CC Voltage Status */
+static int16_t convert_bc_lvl(int16_t port, int16_t bc_lvl)
+{
+	/* assume OPEN unless one of the following conditions is true... */
+	int16_t ret = TYPEC_CC_VOLT_OPEN;
+
+	if (state[port].pulling_up) {
+		if (bc_lvl == 0x00)
+			ret = TYPEC_CC_VOLT_RA;
+		else if (bc_lvl < 0x3)
+			ret = TYPEC_CC_VOLT_RD;
+	} else {
+		if (bc_lvl == 0x1)
+			ret = TYPEC_CC_VOLT_SNK_DEF;
+		else if (bc_lvl == 0x2)
+			ret = TYPEC_CC_VOLT_SNK_1_5;
+		else if (bc_lvl == 0x3)
+			ret = TYPEC_CC_VOLT_SNK_3_0;
+	}
+
+	return ret;
+}
+
+static int16_t measure_cc_pin_source(int16_t port, int16_t cc_measure)
+{
+	int16_t switches0_reg;
+	int16_t reg;
+	int16_t cc_lvl;
+
+	/* Read status register */
+	tcpc_read(port, TCPC_REG_SWITCHES0, &reg);
+	/* Save current value */
+	switches0_reg = reg;
+	/* Clear pull-up register settings and measure bits */
+	reg &= ~(TCPC_REG_SWITCHES0_MEAS_CC1 | TCPC_REG_SWITCHES0_MEAS_CC2);
+	/* Set desired pullup register bit */
+	if (cc_measure == TCPC_REG_SWITCHES0_MEAS_CC1)
+		reg |= TCPC_REG_SWITCHES0_CC1_PU_EN;
+	else
+		reg |= TCPC_REG_SWITCHES0_CC2_PU_EN;
+	/* Set CC measure bit */
+	reg |= cc_measure;
+
+	/* Set measurement switch */
+	tcpc_write(port, TCPC_REG_SWITCHES0, reg);
+
+	/* Set MDAC for Open vs Rd/Ra comparison */
+	tcpc_write(port, TCPC_REG_MEASURE, state[port].mdac_vnc);
+
+	/* Wait on measurement */
+	platform_usleep(250);
+
+	/* Read status register */
+	tcpc_read(port, TCPC_REG_STATUS0, &reg);
+
+	/* Assume open */
+	cc_lvl = TYPEC_CC_VOLT_OPEN;
+
+	/* CC level is below the 'no connect' threshold (vOpen) */
+	if ((reg & TCPC_REG_STATUS0_COMP) == 0) {
+		/* Set MDAC for Rd vs Ra comparison */
+		tcpc_write(port, TCPC_REG_MEASURE, state[port].mdac_rd);
+
+		/* Wait on measurement */
+		platform_usleep(250);
+
+		/* Read status register */
+		tcpc_read(port, TCPC_REG_STATUS0, &reg);
+
+		cc_lvl = (reg & TCPC_REG_STATUS0_COMP) ? TYPEC_CC_VOLT_RD
+		    : TYPEC_CC_VOLT_RA;
+	}
+
+	/* Restore SWITCHES0 register to its value prior */
+	tcpc_write(port, TCPC_REG_SWITCHES0, switches0_reg);
+
+	return cc_lvl;
+}
+
+/* Determine cc pin state for source when in manual detect mode */
+static void detect_cc_pin_source_manual(int16_t port, int16_t *cc1_lvl, int16_t *cc2_lvl)
+{
+	int16_t cc1_measure = TCPC_REG_SWITCHES0_MEAS_CC1;
+	int16_t cc2_measure = TCPC_REG_SWITCHES0_MEAS_CC2;
+
+	if (state[port].vconn_enabled) {
+		/* If VCONN enabled, measure cc_pin that matches polarity */
+		if (state[port].cc_polarity)
+			*cc2_lvl = measure_cc_pin_source(port, cc2_measure);
+		else
+			*cc1_lvl = measure_cc_pin_source(port, cc1_measure);
+	} else {
+		/* If VCONN not enabled, measure both cc1 and cc2 */
+		*cc1_lvl = measure_cc_pin_source(port, cc1_measure);
+		*cc2_lvl = measure_cc_pin_source(port, cc2_measure);
+	}
+
+}
+
+/* Determine cc pin state for sink */
+static void detect_cc_pin_sink(int16_t port, int16_t *cc1, int16_t *cc2)
+{
+	int16_t reg;
+	int16_t orig_meas_cc1;
+	int16_t orig_meas_cc2;
+	int16_t bc_lvl_cc1;
+	int16_t bc_lvl_cc2;
+
+	/*
+	 * Measure CC1 first.
+	 */
+	tcpc_read(port, 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(port, TCPC_REG_SWITCHES0, reg);
+
+	/* CC1 is now being measured by FUSB302. */
+
+	/* Wait on measurement */
+	platform_usleep(250);
+
+	tcpc_read(port, 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(port, 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(port, TCPC_REG_SWITCHES0, reg);
+
+	/* CC2 is now being measured by FUSB302. */
+
+	/* Wait on measurement */
+	platform_usleep(250);
+
+	tcpc_read(port, 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(port, bc_lvl_cc1);
+	*cc2 = convert_bc_lvl(port, bc_lvl_cc2);
+
+	/* return MEAS_CC1/2 switches to original state */
+	tcpc_read(port, 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(port, TCPC_REG_SWITCHES0, reg);
+
+}
+
+/* Parse header bytes for the size of packet */
+static int16_t get_num_bytes(uint16_t header)
+{
+	int16_t rv;
+
+	/* Grab the Number of Data Objects field. */
+	rv = PD_HEADER_CNT(header);
+
+	/* Multiply by four to go from 32-bit words -> bytes */
+	rv *= 4;
+
+	/* Plus 2 for header */
+	rv += 2;
+
+	return rv;
+}
+
+static int16_t fusb302_send_message(int16_t port, uint16_t header,
+				const uint32_t * data, uint8_t * buf,
+				int16_t buf_pos)
+{
+	int16_t rv;
+	int16_t reg;
+	int16_t len;
+
+	len = get_num_bytes(header);
+
+	/*
+	 * packsym tells the TXFIFO that the next X bytes are payload,
+	 * and should not be interpreted as special tokens.
+	 * The 5 LSBs represent X, the number of bytes.
+	 */
+	reg = fusb302_TKN_PACKSYM;
+	reg |= (len & 0x1F);
+
+	buf[buf_pos++] = reg;
+
+	/* write in the header */
+	reg = header;
+	buf[buf_pos++] = reg & 0xFF;
+
+	reg >>= 8;
+	buf[buf_pos++] = reg & 0xFF;
+
+	/* header is done, subtract from length to make this for-loop simpler */
+	len -= 2;
+
+	/* write data objects, if present */
+	memcpy(&buf[buf_pos], data, len);
+	buf_pos += len;
+
+	/* put in the CRC */
+	buf[buf_pos++] = fusb302_TKN_JAMCRC;
+
+	/* put in EOP */
+	buf[buf_pos++] = fusb302_TKN_EOP;
+
+	/* Turn transmitter off after sending message */
+	buf[buf_pos++] = fusb302_TKN_TXOFF;
+
+	/* Start transmission */
+	reg = fusb302_TKN_TXON;
+	buf[buf_pos++] = fusb302_TKN_TXON;
+
+	/* burst write for speed! */
+	rv = tcpc_xfer(port, buf, buf_pos, 0, 0, I2C_XFER_SINGLE);
+
+	return rv;
+}
+
+int16_t fusb302_tcpm_select_rp_value(int16_t port, int16_t rp)
+{
+	int16_t reg;
+	int16_t rv;
+	uint8_t vnc, rd;
+
+	rv = tcpc_read(port, TCPC_REG_CONTROL0, &reg);
+	if (rv)
+		return rv;
+
+	/* Set the current source for Rp value */
+	reg &= ~TCPC_REG_CONTROL0_HOST_CUR_MASK;
+	switch (rp) {
+	case TYPEC_RP_1A5:
+		reg |= TCPC_REG_CONTROL0_HOST_CUR_1A5;
+		vnc = TCPC_REG_MEASURE_MDAC_MV(PD_SRC_1_5_VNC_MV);
+		rd = TCPC_REG_MEASURE_MDAC_MV(PD_SRC_1_5_RD_THRESH_MV);
+		break;
+	case TYPEC_RP_3A0:
+		reg |= TCPC_REG_CONTROL0_HOST_CUR_3A0;
+		vnc = TCPC_REG_MEASURE_MDAC_MV(PD_SRC_3_0_VNC_MV);
+		rd = TCPC_REG_MEASURE_MDAC_MV(PD_SRC_3_0_RD_THRESH_MV);
+		break;
+	case TYPEC_RP_USB:
+	default:
+		reg |= TCPC_REG_CONTROL0_HOST_CUR_USB;
+		vnc = TCPC_REG_MEASURE_MDAC_MV(PD_SRC_DEF_VNC_MV);
+		rd = TCPC_REG_MEASURE_MDAC_MV(PD_SRC_DEF_RD_THRESH_MV);
+	}
+	state[port].mdac_vnc = vnc;
+	state[port].mdac_rd = rd;
+	rv = tcpc_write(port, TCPC_REG_CONTROL0, reg);
+
+	return rv;
+}
+
+int16_t fusb302_tcpm_init(int16_t port)
+{
+	int16_t reg;
+
+	/* set default */
+	state[port].cc_polarity = -1;
+
+	/* set the voltage threshold for no connect detection (vOpen) */
+	state[port].mdac_vnc = TCPC_REG_MEASURE_MDAC_MV(PD_SRC_DEF_VNC_MV);
+	/* set the voltage threshold for Rd vs Ra detection */
+	state[port].mdac_rd = TCPC_REG_MEASURE_MDAC_MV(PD_SRC_DEF_RD_THRESH_MV);
+
+	/* all other variables assumed to default to 0 */
+
+	/* Restore default settings */
+	tcpc_write(port, TCPC_REG_RESET, TCPC_REG_RESET_SW_RESET);
+
+	tcpc_read(port, TCPC_REG_DEVICE_ID, &reg);
+
+	/* Turn on retries and set number of retries */
+	tcpc_read(port, TCPC_REG_CONTROL3, &reg);
+	reg |= TCPC_REG_CONTROL3_AUTO_RETRY;
+	reg |= (PD_RETRY_COUNT & 0x3) << TCPC_REG_CONTROL3_N_RETRIES_POS;
+	tcpc_write(port, TCPC_REG_CONTROL3, reg);
+
+	/* Create interrupt masks */
+	reg = 0xFF;
+	/* VBUS OK */
+	reg &= ~TCPC_REG_MASK_VBUSOK;
+	/* CC level changes */
+	reg &= ~TCPC_REG_MASK_BC_LVL;
+	/* collisions */
+	reg &= ~TCPC_REG_MASK_COLLISION;
+	/* misc alert */
+	reg &= ~TCPC_REG_MASK_ALERT;
+	/* packet received with correct CRC */
+	reg &= ~TCPC_REG_MASK_CRC_CHK;
+	tcpc_write(port, TCPC_REG_MASK, reg);
+
+	reg = 0xFF;
+	/* when all pd message retries fail... */
+	reg &= ~TCPC_REG_MASKA_RETRYFAIL;
+	/* when fusb302 send a hard reset. */
+	reg &= ~TCPC_REG_MASKA_HARDSENT;
+	/* when fusb302 receives GoodCRC ack for a pd message */
+	reg &= ~TCPC_REG_MASKA_TX_SUCCESS;
+	/* when fusb302 receives a hard reset */
+	reg &= ~TCPC_REG_MASKA_HARDRESET;
+	tcpc_write(port, TCPC_REG_MASKA, reg);
+
+	reg = 0xFF;
+	/* when fusb302 sends GoodCRC to ack a pd message */
+	reg &= ~TCPC_REG_MASKB_GCRCSENT;
+	tcpc_write(port, TCPC_REG_MASKB, reg);
+
+	/* Interrupt Enable */
+	tcpc_read(port, TCPC_REG_CONTROL0, &reg);
+	reg &= ~TCPC_REG_CONTROL0_INT_MASK;
+	tcpc_write(port, TCPC_REG_CONTROL0, reg);
+
+	state[port].control1 =
+	    TCPC_REG_CONTROL1_RX_FLUSH | TCPC_REG_CONTROL1_ENSOP1DB |
+	    TCPC_REG_CONTROL1_ENSOP2DB;
+	tcpc_write(port, TCPC_REG_CONTROL1, state[port].control1);
+
+	fusb302_auto_goodcrc_enable(port, 0);
+
+	/* Turn on the power! */
+	/* TODO: Reduce power consumption */
+	tcpc_write(port, TCPC_REG_POWER, TCPC_REG_POWER_PWR_ALL);
+
+	return 0;
+}
+
+int16_t fusb302_tcpm_get_cc(int16_t port, int16_t *cc1, int16_t *cc2)
+{
+	if (state[port].pulling_up) {
+		/* Source mode? */
+		detect_cc_pin_source_manual(port, cc1, cc2);
+	} else {
+		/* Sink mode? */
+		detect_cc_pin_sink(port, cc1, cc2);
+	}
+
+	return 0;
+}
+
+int16_t fusb302_tcpm_set_cc(int16_t port, int16_t pull)
+{
+	int16_t reg;
+
+	/* NOTE: FUSB302 toggles a single pull-up between CC1 and CC2 */
+	/* NOTE: FUSB302 Does not support Ra. */
+	switch (pull) {
+	case TYPEC_CC_RP:
+		/* enable the pull-up we know to be necessary */
+		tcpc_read(port, TCPC_REG_SWITCHES0, &reg);
+
+		reg &= ~(TCPC_REG_SWITCHES0_CC2_PU_EN |
+			 TCPC_REG_SWITCHES0_CC1_PU_EN |
+			 TCPC_REG_SWITCHES0_CC1_PD_EN |
+			 TCPC_REG_SWITCHES0_CC2_PD_EN |
+			 TCPC_REG_SWITCHES0_VCONN_CC1 |
+			 TCPC_REG_SWITCHES0_VCONN_CC2);
+
+		reg |= TCPC_REG_SWITCHES0_CC1_PU_EN |
+		    TCPC_REG_SWITCHES0_CC2_PU_EN;
+
+		if (state[port].vconn_enabled)
+			reg |= state[port].cc_polarity ?
+			    TCPC_REG_SWITCHES0_VCONN_CC1 :
+			    TCPC_REG_SWITCHES0_VCONN_CC2;
+
+		tcpc_write(port, TCPC_REG_SWITCHES0, reg);
+
+		state[port].pulling_up = 1;
+		break;
+	case TYPEC_CC_RD:
+		/* Enable UFP Mode */
+
+		/* turn off toggle */
+		tcpc_read(port, TCPC_REG_CONTROL2, &reg);
+		reg &= ~TCPC_REG_CONTROL2_TOGGLE;
+		tcpc_write(port, TCPC_REG_CONTROL2, reg);
+
+		/* enable pull-downs, disable pullups */
+		tcpc_read(port, TCPC_REG_SWITCHES0, &reg);
+
+		reg &= ~(TCPC_REG_SWITCHES0_CC2_PU_EN);
+		reg &= ~(TCPC_REG_SWITCHES0_CC1_PU_EN);
+		reg |= (TCPC_REG_SWITCHES0_CC1_PD_EN);
+		reg |= (TCPC_REG_SWITCHES0_CC2_PD_EN);
+		tcpc_write(port, TCPC_REG_SWITCHES0, reg);
+
+		state[port].pulling_up = 0;
+		break;
+	case TYPEC_CC_OPEN:
+		/* Disable toggling */
+		tcpc_read(port, TCPC_REG_CONTROL2, &reg);
+		reg &= ~TCPC_REG_CONTROL2_TOGGLE;
+		tcpc_write(port, TCPC_REG_CONTROL2, reg);
+
+		/* Ensure manual switches are opened */
+		tcpc_read(port, TCPC_REG_SWITCHES0, &reg);
+		reg &= ~TCPC_REG_SWITCHES0_CC1_PU_EN;
+		reg &= ~TCPC_REG_SWITCHES0_CC2_PU_EN;
+		reg &= ~TCPC_REG_SWITCHES0_CC1_PD_EN;
+		reg &= ~TCPC_REG_SWITCHES0_CC2_PD_EN;
+		tcpc_write(port, TCPC_REG_SWITCHES0, reg);
+
+		state[port].pulling_up = 0;
+		break;
+	default:
+		/* Unsupported... */
+		return EC_ERROR_UNIMPLEMENTED;
+	}
+
+	return 0;
+}
+
+int16_t fusb302_tcpm_set_polarity(int16_t port, int16_t polarity)
+{
+	/* Port polarity : 0 => CC1 is CC line, 1 => CC2 is CC line */
+	int16_t reg;
+
+	tcpc_read(port, TCPC_REG_SWITCHES0, &reg);
+
+	/* clear VCONN switch bits */
+	reg &= ~TCPC_REG_SWITCHES0_VCONN_CC1;
+	reg &= ~TCPC_REG_SWITCHES0_VCONN_CC2;
+
+	if (state[port].vconn_enabled) {
+		/* set VCONN switch to be non-CC line */
+		if (polarity)
+			reg |= TCPC_REG_SWITCHES0_VCONN_CC1;
+		else
+			reg |= TCPC_REG_SWITCHES0_VCONN_CC2;
+	}
+
+	/* clear meas_cc bits (RX line select) */
+	reg &= ~TCPC_REG_SWITCHES0_MEAS_CC1;
+	reg &= ~TCPC_REG_SWITCHES0_MEAS_CC2;
+
+	/* set rx polarity */
+	if (polarity)
+		reg |= TCPC_REG_SWITCHES0_MEAS_CC2;
+	else
+		reg |= TCPC_REG_SWITCHES0_MEAS_CC1;
+
+	tcpc_write(port, TCPC_REG_SWITCHES0, reg);
+
+	tcpc_read(port, TCPC_REG_SWITCHES1, &reg);
+
+	/* clear tx_cc bits */
+	reg &= ~TCPC_REG_SWITCHES1_TXCC1_EN;
+	reg &= ~TCPC_REG_SWITCHES1_TXCC2_EN;
+
+	/* set tx polarity */
+	if (polarity)
+		reg |= TCPC_REG_SWITCHES1_TXCC2_EN;
+	else
+		reg |= TCPC_REG_SWITCHES1_TXCC1_EN;
+
+	tcpc_write(port, TCPC_REG_SWITCHES1, reg);
+
+	/* Save the polarity for later */
+	state[port].cc_polarity = polarity;
+
+	return 0;
+}
+
+int16_t fusb302_tcpm_set_msg_header(int16_t port, int16_t power_role, int16_t data_role)
+{
+	int16_t reg;
+
+	tcpc_read(port, TCPC_REG_SWITCHES1, &reg);
+
+	reg &= ~TCPC_REG_SWITCHES1_POWERROLE;
+	reg &= ~TCPC_REG_SWITCHES1_DATAROLE;
+
+	if (power_role)
+		reg |= TCPC_REG_SWITCHES1_POWERROLE;
+	if (data_role)
+		reg |= TCPC_REG_SWITCHES1_DATAROLE;
+
+	tcpc_write(port, TCPC_REG_SWITCHES1, reg);
+
+	return 0;
+}
+
+int16_t fusb302_tcpm_set_rx_enable(int16_t port, int16_t enable)
+{
+	int16_t reg;
+
+	state[port].rx_enable = enable;
+
+	/* Get current switch state */
+	tcpc_read(port, TCPC_REG_SWITCHES0, &reg);
+
+	/* Clear CC1/CC2 measure bits */
+	reg &= ~TCPC_REG_SWITCHES0_MEAS_CC1;
+	reg &= ~TCPC_REG_SWITCHES0_MEAS_CC2;
+
+	if (enable) {
+		switch (state[port].cc_polarity) {
+			/* if CC polarity hasnt been determined, can't enable */
+		case -1:
+			return EC_ERROR_UNKNOWN;
+		case 0:
+			reg |= TCPC_REG_SWITCHES0_MEAS_CC1;
+			break;
+		case 1:
+			reg |= TCPC_REG_SWITCHES0_MEAS_CC2;
+			break;
+		default:
+			/* "shouldn't get here" */
+			return EC_ERROR_UNKNOWN;
+		}
+		tcpc_write(port, TCPC_REG_SWITCHES0, reg);
+
+		/* Disable BC_LVL interrupt when enabling PD comm */
+		if (!tcpc_read(port, TCPC_REG_MASK, &reg))
+			tcpc_write(port, TCPC_REG_MASK,
+				   reg | TCPC_REG_MASK_BC_LVL);
+
+		/* flush rx fifo in case messages have been coming our way */
+		fusb302_flush_rx_fifo(port);
+
+	} else {
+		tcpc_write(port, TCPC_REG_SWITCHES0, reg);
+
+		/* Enable BC_LVL interrupt when disabling PD comm */
+		if (!tcpc_read(port, TCPC_REG_MASK, &reg))
+			tcpc_write(port, TCPC_REG_MASK,
+				   reg & ~TCPC_REG_MASK_BC_LVL);
+	}
+
+	fusb302_auto_goodcrc_enable(port, enable);
+
+	return 0;
+}
+
+/* Return true if our Rx FIFO is empty */
+int16_t fusb302_rx_fifo_is_empty(int16_t port)
+{
+	int16_t reg, ret;
+
+	ret = (!tcpc_read(port, TCPC_REG_STATUS1, &reg)) &&
+	    (reg & TCPC_REG_STATUS1_RX_EMPTY);
+
+	return ret;
+}
+
+int16_t fusb302_tcpm_get_message(int16_t port, uint32_t * payload, int16_t *head,
+			     enum fusb302_rxfifo_tokens *sop)
+{
+	/*
+	 * This is the buffer that will get the burst-read data
+	 * from the fusb302.
+	 *
+	 * It's re-used in a couple different spots, the worst of which
+	 * is the PD packet (not header) and CRC.
+	 * maximum size necessary = 28 + 4 = 32
+	 */
+	uint8_t buf[32];
+	int16_t rv, len;
+
+	/* If our FIFO is empty then we have no packet */
+	if (fusb302_rx_fifo_is_empty(port))
+		return EC_ERROR_UNKNOWN;
+
+	/* Read until we have a non-GoodCRC packet or an empty FIFO */
+	do {
+		buf[0] = TCPC_REG_FIFOS;
+
+		/*
+		 * PART 1 OF BURST READ: Write in register address.
+		 * Issue a START, no STOP.
+		 */
+		rv = tcpc_xfer(port, buf, 1, 0, 0, I2C_XFER_START);
+
+		/*
+		 * PART 2 OF BURST READ: Read up to the header.
+		 * Issue a repeated START, no STOP.
+		 * only grab three bytes so we can get the header
+		 * and determine how many more bytes we need to read.
+		 * TODO: Check token to ensure valid packet.
+		 */
+		rv |= tcpc_xfer(port, 0, 0, buf, 3, I2C_XFER_START);
+
+		/* Grab the header */
+		*sop = buf[0] & fusb302_TKN_SOP_MASK;
+		*head = (buf[1] & 0xFF);
+		*head |= ((buf[2] << 8) & 0xFF00);
+
+		/* figure out packet length, subtract header bytes */
+		len = get_num_bytes(*head) - 2;
+
+		/*
+		 * PART 3 OF BURST READ: Read everything else.
+		 * No START, but do issue a STOP at the end.
+		 * add 4 to len to read CRC out
+		 */
+		rv |= tcpc_xfer(port, 0, 0, buf, len + 4, I2C_XFER_STOP);
+
+	} while (!rv && PACKET_IS_GOOD_CRC(*head) &&
+		 !fusb302_rx_fifo_is_empty(port));
+
+	if (!rv) {
+		/* Discard GoodCRC packets */
+		if (PACKET_IS_GOOD_CRC(*head))
+			rv = EC_ERROR_UNKNOWN;
+		else
+			memcpy(payload, buf, len);
+	}
+
+	/*
+	 * If our FIFO is non-empty then we may have a packet, we may get
+	 * fewer interrupts than packets due to interrupt latency.
+	 */
+	//if (!fusb302_rx_fifo_is_empty(port))
+	//  task_set_event(PD_PORT_TO_TASK_ID(port), PD_EVENT_RX, 0);
+
+	return rv;
+}
+
+int16_t fusb302_tcpm_transmit(int16_t port, enum tcpm_transmit_type type,
+			  uint16_t header, const uint32_t * data)
+{
+	/*
+	 * this is the buffer that will be burst-written into the fusb302
+	 * maximum size necessary =
+	 * 1: FIFO register address
+	 * 4: SOP* tokens
+	 * 1: Token that signifies "next X bytes are not tokens"
+	 * 30: 2 for header and up to 7*4 = 28 for rest of message
+	 * 1: "Insert CRC" Token
+	 * 1: EOP Token
+	 * 1: "Turn transmitter off" token
+	 * 1: "Star Transmission" Command
+	 * -
+	 * 40: 40 bytes worst-case
+	 */
+	uint8_t buf[40];
+	int16_t buf_pos = 0;
+
+	int16_t reg;
+
+	/* Flush the TXFIFO */
+	fusb302_flush_tx_fifo(port);
+
+	header |= state[port].msgid++ << 9;
+	state[port].msgid &= 0x7;
+
+	switch (type) {
+	case TCPC_TX_SOP:
+
+		/* put register address first for of burst tcpc write */
+		buf[buf_pos++] = TCPC_REG_FIFOS;
+
+		/* Write the SOP Ordered Set into TX FIFO */
+		buf[buf_pos++] = fusb302_TKN_SYNC1;
+		buf[buf_pos++] = fusb302_TKN_SYNC1;
+		buf[buf_pos++] = fusb302_TKN_SYNC1;
+		buf[buf_pos++] = fusb302_TKN_SYNC2;
+
+		fusb302_send_message(port, header, data, buf, buf_pos);
+		// wait for the GoodCRC to come back before we let the rest
+		// of the code do stuff like change polarity and miss it
+		platform_usleep(1200);
+		return 0;
+	case TCPC_TX_SOP_PRIME:
+
+		/* put register address first for of burst tcpc write */
+		buf[buf_pos++] = TCPC_REG_FIFOS;
+
+		/* Write the SOP Ordered Set into TX FIFO */
+		buf[buf_pos++] = fusb302_TKN_SYNC1;
+		buf[buf_pos++] = fusb302_TKN_SYNC1;
+		buf[buf_pos++] = fusb302_TKN_SYNC3;
+		buf[buf_pos++] = fusb302_TKN_SYNC3;
+
+		fusb302_send_message(port, header, data, buf, buf_pos);
+		// wait for the GoodCRC to come back before we let the rest
+		// of the code do stuff like change polarity and miss it
+		platform_usleep(1200);
+		return 0;
+	case TCPC_TX_SOP_PRIME_PRIME:
+
+		/* put register address first for of burst tcpc write */
+		buf[buf_pos++] = TCPC_REG_FIFOS;
+
+		/* Write the SOP Ordered Set into TX FIFO */
+		buf[buf_pos++] = fusb302_TKN_SYNC1;
+		buf[buf_pos++] = fusb302_TKN_SYNC3;
+		buf[buf_pos++] = fusb302_TKN_SYNC1;
+		buf[buf_pos++] = fusb302_TKN_SYNC3;
+
+		fusb302_send_message(port, header, data, buf, buf_pos);
+		// wait for the GoodCRC to come back before we let the rest
+		// of the code do stuff like change polarity and miss it
+		platform_usleep(1200);
+		return 0;
+	case TCPC_TX_SOP_DEBUG_PRIME:
+
+		/* put register address first for of burst tcpc write */
+		buf[buf_pos++] = TCPC_REG_FIFOS;
+
+		/* Write the SOP Ordered Set into TX FIFO */
+		buf[buf_pos++] = fusb302_TKN_SYNC1;
+		buf[buf_pos++] = fusb302_TKN_RST2;
+		buf[buf_pos++] = fusb302_TKN_RST2;
+		buf[buf_pos++] = fusb302_TKN_SYNC3;
+
+		fusb302_send_message(port, header, data, buf, buf_pos);
+		// wait for the GoodCRC to come back before we let the rest
+		// of the code do stuff like change polarity and miss it
+		platform_usleep(1200);
+		return 0;
+	case TCPC_TX_SOP_DEBUG_PRIME_PRIME:
+
+		/* put register address first for of burst tcpc write */
+		buf[buf_pos++] = TCPC_REG_FIFOS;
+
+		/* Write the SOP Ordered Set into TX FIFO */
+		buf[buf_pos++] = fusb302_TKN_SYNC1;
+		buf[buf_pos++] = fusb302_TKN_RST2;
+		buf[buf_pos++] = fusb302_TKN_SYNC3;
+		buf[buf_pos++] = fusb302_TKN_SYNC2;
+
+		fusb302_send_message(port, header, data, buf, buf_pos);
+		// wait for the GoodCRC to come back before we let the rest
+		// of the code do stuff like change polarity and miss it
+		platform_usleep(1200);
+		return 0;
+	case TCPC_TX_HARD_RESET:
+		/* Simply hit the SEND_HARD_RESET bit */
+		tcpc_read(port, TCPC_REG_CONTROL3, &reg);
+		reg |= TCPC_REG_CONTROL3_SEND_HARDRESET;
+		tcpc_write(port, TCPC_REG_CONTROL3, reg);
+
+		break;
+	case TCPC_TX_BIST_MODE_2:
+		/* Hit the BIST_MODE2 bit and start TX */
+		tcpc_read(port, TCPC_REG_CONTROL1, &reg);
+		reg |= TCPC_REG_CONTROL1_BIST_MODE2;
+		tcpc_write(port, TCPC_REG_CONTROL1, reg);
+
+		tcpc_read(port, TCPC_REG_CONTROL0, &reg);
+		reg |= TCPC_REG_CONTROL0_TX_START;
+		tcpc_write(port, TCPC_REG_CONTROL0, reg);
+
+		//task_wait_event(PD_T_BIST_TRANSMIT);
+
+		/* Clear BIST mode bit, TX_START is self-clearing */
+		tcpc_read(port, TCPC_REG_CONTROL1, &reg);
+		reg &= ~TCPC_REG_CONTROL1_BIST_MODE2;
+		tcpc_write(port, TCPC_REG_CONTROL1, reg);
+
+		break;
+	default:
+		return EC_ERROR_UNIMPLEMENTED;
+	}
+
+	return 0;
+}
+
+int16_t fusb302_tcpm_get_vbus_level(int16_t port)
+{
+	int16_t reg;
+
+	/* Read status register */
+	tcpc_read(port, TCPC_REG_STATUS0, &reg);
+
+	return (reg & TCPC_REG_STATUS0_VBUSOK) ? 1 : 0;
+}
+
+void fusb302_get_irq(int16_t port, int16_t *interrupt, int16_t *interrupta, int16_t *interruptb)
+{
+	/* reading interrupt registers clears them */
+
+	tcpc_read(port, TCPC_REG_INTERRUPT, interrupt);
+	tcpc_read(port, TCPC_REG_INTERRUPTA, interrupta);
+	tcpc_read(port, TCPC_REG_INTERRUPTB, interruptb);
+
+#if 0
+	/*
+	 * Ignore BC_LVL changes when transmitting / receiving PD,
+	 * since CC level will constantly change.
+	 */
+	if (state[port].rx_enable)
+		interrupt &= ~TCPC_REG_INTERRUPT_BC_LVL;
+
+	if (interrupt & TCPC_REG_INTERRUPT_BC_LVL) {
+		/* CC Status change */
+		//task_set_event(PD_PORT_TO_TASK_ID(port), PD_EVENT_CC, 0);
+	}
+
+	if (interrupt & TCPC_REG_INTERRUPT_COLLISION) {
+		/* packet sending collided */
+		pd_transmit_complete(port, TCPC_TX_COMPLETE_FAILED);
+	}
+
+	/* GoodCRC was received, our FIFO is now non-empty */
+	if (interrupta & TCPC_REG_INTERRUPTA_TX_SUCCESS) {
+		//task_set_event(PD_PORT_TO_TASK_ID(port),
+		//    PD_EVENT_RX, 0);
+
+		pd_transmit_complete(port, TCPC_TX_COMPLETE_SUCCESS);
+	}
+
+	if (interrupta & TCPC_REG_INTERRUPTA_RETRYFAIL) {
+		/* all retries have failed to get a GoodCRC */
+		pd_transmit_complete(port, TCPC_TX_COMPLETE_FAILED);
+	}
+
+	if (interrupta & TCPC_REG_INTERRUPTA_HARDSENT) {
+		/* hard reset has been sent */
+
+		/* bring FUSB302 out of reset */
+		fusb302_pd_reset(port);
+
+		pd_transmit_complete(port, TCPC_TX_COMPLETE_SUCCESS);
+	}
+
+	if (interrupta & TCPC_REG_INTERRUPTA_HARDRESET) {
+		/* hard reset has been received */
+
+		/* bring FUSB302 out of reset */
+		fusb302_pd_reset(port);
+
+		pd_execute_hard_reset(port);
+
+		//task_wake(PD_PORT_TO_TASK_ID(port));
+	}
+
+	if (interruptb & TCPC_REG_INTERRUPTB_GCRCSENT) {
+		/* Packet received and GoodCRC sent */
+		/* (this interrupt fires after the GoodCRC finishes) */
+		if (state[port].rx_enable) {
+			//task_set_event(PD_PORT_TO_TASK_ID(port),
+			//    PD_EVENT_RX, 0);
+		} else {
+			/* flush rx fifo if rx isn't enabled */
+			fusb302_flush_rx_fifo(port);
+		}
+	}
+#endif
+}
+
+#if 0
+/* For BIST receiving */
+void tcpm_set_bist_test_data(int16_t port)
+{
+	int16_t reg;
+
+	/* Read control3 register */
+	tcpc_read(port, TCPC_REG_CONTROL3, &reg);
+
+	/* Set the BIST_TMODE bit (Clears on Hard Reset) */
+	reg |= TCPC_REG_CONTROL3_BIST_TMODE;
+
+	/* Write the updated value */
+	tcpc_write(port, TCPC_REG_CONTROL3, reg);
+}
+#endif
diff --git a/FUSB302.h b/FUSB302.h
new file mode 100644
index 0000000..0e26a7a
--- /dev/null
+++ b/FUSB302.h
@@ -0,0 +1,248 @@
+/*
+  FUSB302.h - Library for interacting with the FUSB302B chip.
+  Copyright 2010 The Chromium OS Authors
+  Copyright 2017 Jason Cerundolo
+  Released under an MIT license. See LICENSE file. 
+*/
+
+#ifndef fusb302_H
+#define fusb302_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include "usb_pd_tcpm.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 0x22 // 7-bit address for Arduino
+/* FUSB302B01MPX */
+#define fusb302_I2C_SLAVE_ADDR_B01 0x23
+/* FUSB302B10MPX */
+#define fusb302_I2C_SLAVE_ADDR_B10 0x24
+/* FUSB302B11MPX */
+#define fusb302_I2C_SLAVE_ADDR_B11 0x25
+
+/* Default retry count for transmitting */
+#define PD_RETRY_COUNT      3
+
+/* Time to wait for TCPC to complete transmit */
+#define PD_T_TCPC_TX_TIMEOUT  (100*MSEC_US)
+
+#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_VBUS       (1<<6)
+#define TCPC_REG_MEASURE_MDAC_MV(mv)    (((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      (1<<1)
+#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,
+};
+
+enum fusb302_rxfifo_tokens {
+    fusb302_TKN_SOP = 0xE0,
+    fusb302_TKN_SOP1 = 0xC0,
+    fusb302_TKN_SOP2 = 0xA0,
+    fusb302_TKN_SOP1DB = 0x80,
+    fusb302_TKN_SOP2DB = 0x60,
+    fusb302_TKN_SOP_MASK = 0xE0,
+};
+
+extern const struct tcpm_drv fusb302_tcpm_drv;
+
+// Common methods for TCPM implementations
+int16_t fusb302_tcpm_init(int16_t port);
+void fusb302_pd_reset(int16_t port);
+void fusb302_flush_rx_fifo(int16_t port);
+void fusb302_flush_tx_fifo(int16_t port);
+void fusb302_auto_goodcrc_enable(int16_t port, int16_t enable);
+int16_t fusb302_tcpm_get_cc(int16_t port, int16_t *cc1, int16_t *cc2);
+int16_t fusb302_tcpm_set_cc(int16_t port, int16_t pull);
+int16_t fusb302_tcpm_set_polarity(int16_t port, int16_t polarity);
+int16_t fusb302_tcpm_set_vconn(int16_t port, int16_t enable);
+int16_t fusb302_tcpm_set_msg_header(int16_t port, int16_t power_role, int16_t data_role);
+int16_t fusb302_tcpm_set_rx_enable(int16_t port, int16_t enable);
+int16_t fusb302_tcpm_get_message(int16_t port, uint32_t *payload, int16_t *head, enum fusb302_rxfifo_tokens *sop);
+int16_t fusb302_tcpm_transmit(int16_t port, enum tcpm_transmit_type type, uint16_t header, const uint32_t *data);
+int16_t fusb302_tcpm_get_vbus_level(int16_t port);
+int16_t fusb302_tcpm_select_rp_value(int16_t port, int16_t rp);
+void fusb302_get_irq(int16_t port, int16_t *irq, int16_t *irqa, int16_t *irqb);
+int16_t fusb302_rx_fifo_is_empty(int16_t port);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* fusb302_H */
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..d45df74
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,24 @@
+MIT License
+
+Copyright (c) 2014 The Chromium OS Authors
+Copyright (c) 2018 Reclaimer Labs - https://www.reclaimerlabs.com/
+Copyright (c) 2020 The Asahi Linux Contributors
+Copyright (c) 2022 Marc Zyngier
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/README.txt b/README.txt
new file mode 100644
index 0000000..686cac2
--- /dev/null
+++ b/README.txt
@@ -0,0 +1,189 @@
+"This is the Central Scrutinizer"
+
+So you have built (or otherwise obtained) a device labelled "Central
+Scrutinizer" which allows you interact with the serial port of a M1/M2
+and reboot it.
+
+This file describe how to build the software and flash it onto the
+Raspberry-Pi Pico that controls the adapter. As for the HW, the SW
+started its life as m1-ubmc, but let's be clear that its name really
+is "Central Scrutinizer" so that you too can channel your inner FZ.
+
+But first, credit where credit is due: this software is not an
+original work, but is built on top of code which is:
+
+  Copyright (c) 2014 The Chromium OS Authors
+  Copyright (c) 2018 Reclaimer Labs - https://www.reclaimerlabs.com/
+  Copyright (c) 2020 The Asahi Linux Contributors
+
+See the LICENSE file for the fine print.
+
+** Install the pre-requisites
+
+On a Debian system, this is what you need:
+
+  sudo apt install cmake gcc-arm-none-eabi build-essential git
+
+I'm sure there is an equivalent for other OSs, but life is too short
+to use anything else.
+
+** Install the pico-sdk:
+
+I've mostly used version 1.4 of the SDK, but 1.5 seems to work
+too. YMMV.
+
+  git clone -b master https://github.com/raspberrypi/pico-sdk.git
+
+  export PICO_SDK_PATH=/the/path/to/pico-sdk
+
+  cd pico-sdk
+
+  git submodule update --init
+
+** Build the Pico firmware:
+
+It builds just like any other pico-sdk project:
+
+  git clone git://git.kernel.org/pub/scm/linux/kernel/git/maz/cs-sw
+
+  cd cs-sw
+
+  mkdir build
+
+  cd build
+
+  cmake ..
+
+  make
+
+You should end-up with a file called m1_ubmc.uf2 in the build
+directory. If you don't, something is wrong. Finding what is wrong is
+your responsibility, not mine! ;-)
+
+** Flash it
+
+Place the Pico in programming mode by pressing the BOOTROM button
+while plugging the USB connector, and issue something along the lines
+of:
+
+  sudo mount /dev/disk/by-id/usb-RPI_RP2_E0C9125B0D9B-0\:0-part1 /mnt
+
+  sudo cp m1_ubmc.uf2 /mnt
+
+  sudo eject /mnt
+
+** Plug it
+  
+You will need a cable that contains most (if not all) of the USB-C
+wires, and crucially the SBU signals. Cheap cables won't carry them,
+but you won't find out until you actually try them.
+
+You really want something that looks thick and stiff, and flimsy
+cables are unlikely work (the cable that ships with M1 laptops
+doesn't). I've had good results with cables designed to carry video
+signals.
+
+Because I'm lazy, the hardware only connects a single CC line to the
+board's PD controller. Which means that on the board side, there is
+only a single valid orientation for the USB-C cable. Trial and error
+are, as usual, your best friends. Put a label on the board's end of
+the cable as an indication of the orientation.
+
+** Use it
+
+If you have correctly built and flashed the firmware, you will have
+the Pico led blinking at the rate of twice a second, and a
+/dev/ttyACM0 (or similar) that was detected by your host:
+
+  [420294.546630] usb 1-4: USB disconnect, device number 12
+  [420294.902512] usb 1-4: new full-speed USB device number 13 using xhci_hcd
+  [420295.051407] usb 1-4: New USB device found, idVendor=2e8a, idProduct=000a, bcdDevice= 1.00
+  [420295.051421] usb 1-4: New USB device strings: Mfr=1, Product=2, SerialNumber=3
+  [420295.051427] usb 1-4: Product: Pico
+  [420295.051431] usb 1-4: Manufacturer: Raspberry Pi
+  [420295.051434] usb 1-4: SerialNumber: E66164084319392A
+  [420295.054182] cdc_acm 1-4:1.0: ttyACM0: USB ACM device
+
+The board identifies itself as a Pico, not the Central Scrutinizer you
+were hoping for. Again, I'm lazy. Who cares?
+
+Just run:
+
+  screen /dev/ttyACM0
+
+and you should see something like:
+
+  This is the Central Scrutinizer
+  Control character is ^_
+  Press ^_ + ? for help
+  P0: VBUS OFF
+  P0: Device ID: 0x91
+  P0: Init
+  P0: STATUS0: 0x80
+  P0: VBUS OFF
+  P0: Disconnected
+  P0: S: DISCONNECTED
+  P0: Empty debug message
+  P1: I2C pins low while idling, skipping port
+  P0: IRQ=0 10 0
+  P0: Connected: cc1=2 cc2=0
+  P0: Polarity: CC1 (normal)
+  P0: VBUS ON
+  P0: S: DFP_VBUS_ON
+  P0: Empty debug message
+  P0: IRQ=71 4 0
+  P0: S: DFP_CONNECTED
+  P0: >VDM serial -> SBU1/2
+  P0: IRQ=71 4 0
+
+If you see the ">VDM serial -> SBU1/2" line, the serial line should
+now be connected and you can interact with the M1. Note that you can
+use any serial configuration you want on the Mac side as long as it is
+115200n8. One day I may implement the required controls, but that's
+super low priority on the list of things I want to do. Also, there is
+no such list, and the current setup works well enough for me.
+
+Typing ^_? (Control-Underscore followed by a question mark) will lead
+to the follwing dump:
+
+  ^_    Escape character
+  ^_ ^_ Raw ^_
+  ^_ ^@ Send break
+  ^_ !  DUT reset
+  ^_ ^R Central Scrutinizer reset
+  ^_ ^^ Central Scrutinizer reset to programming mode
+  ^_ ^D Toggle debug
+  ^_ ^M Send empty debug VDM
+  ^_ ?  This message
+  P0: present
+  P1: absent
+  
+which is completely self explainatory, but let's expand on it anyway:
+
+- ^_ ^_ sends a raw ^_, just in case you really need it
+
+- ^_ ^@ sends a break, which is useful if interacting with a Linux
+  console as you get the sysrq functionality.
+
+- ^_ ! resets the Mac without any warning. Yes, this is dangerous, use
+  with caution and only when nothing else will do.
+
+- ^_ ^R reboots the Central Scrutinizer itself. Not very useful, except
+  when it is.
+
+- ^_ ^^ reboots the Central Scrutinizer in programming mode, exactly
+  as if you had plugged it with the BOOTROM button pressed. You end-up
+  in mass-storage mode and can update the firmware.
+
+- ^_ ^D toggles the "debug" internal flag. This has very little effect
+  at the moment as most of the debug statements ignore the flag and
+  spit out the junk on the console unconditionally.
+
+- ^_ ^M sends an empty debug message to the remote PD controller.
+  That's a debug feature...
+
+- ^_ ? prints the help message (duh).
+
+Finally, the P0:/P1: lines indicate which I2C/UART combination the
+board is using. The HW supports two boards being driven by a single
+Pico, but the SW is only vaguely aware of it. WIP.
diff --git a/m1-pd-bmc.h b/m1-pd-bmc.h
new file mode 100644
index 0000000..3ccf5a0
--- /dev/null
+++ b/m1-pd-bmc.h
@@ -0,0 +1,42 @@
+// FUSB302-based serial/reset/whatever controller for M1-based systems
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "pico/stdlib.h"
+
+#include "hardware/gpio.h"
+#include "hardware/i2c.h"
+
+struct gpio_pin_config {
+	uint16_t		pin;
+	enum gpio_function	mode;
+	int			dir;
+	bool			pu;
+	bool			skip;
+};
+
+enum m1_pd_bmc_pins {
+	LED_G,
+	I2C_SDA,
+	I2C_SCL,
+	FUSB_INT,
+	FUSB_VBUS,
+	UART_TX,
+	UART_RX,
+};
+
+struct hw_context {
+	const struct gpio_pin_config 	*pins;
+	uart_inst_t 			*const uart;
+	i2c_inst_t			*const i2c;
+	void				(*uart_handler)(void);
+	uint8_t		 		addr;
+	uint8_t				nr_pins;
+	uint8_t				uart_irq;
+};
+
+const struct hw_context *get_hw_from_port(int port);
+void m1_pd_bmc_fusb_setup(unsigned int port,
+			  const struct hw_context *hw);
+void m1_pd_bmc_run(void);
diff --git a/platform.h b/platform.h
new file mode 100644
index 0000000..b23d20d
--- /dev/null
+++ b/platform.h
@@ -0,0 +1,9 @@
+#include <stdio.h>
+#include "m1-pd-bmc.h"
+
+static void platform_usleep(uint64_t us)
+{
+	sleep_ms(us / 1000);
+	sleep_us(us % 1000);
+}
+
diff --git a/start.c b/start.c
new file mode 100644
index 0000000..fecd022
--- /dev/null
+++ b/start.c
@@ -0,0 +1,186 @@
+// FUSB302-based serial/reset/whatever controller for M1-based systems
+
+#include <stddef.h>
+#include <stdio.h>
+
+#include "m1-pd-bmc.h"
+#include "FUSB302.h"
+
+static const struct gpio_pin_config m1_pd_bmc_pin_config0[] = {
+	[LED_G] = {
+		.pin	= 25,
+		.mode	= GPIO_FUNC_SIO,
+		.dir	= GPIO_OUT,
+	},
+	[I2C_SDA] = {		/* I2C0 */
+		.pin	= 16,
+		.mode	= GPIO_FUNC_I2C,
+	},
+	[I2C_SCL] = {		/* I2C0 */
+		.pin	= 17,
+		.mode	= GPIO_FUNC_I2C,
+	},
+	[FUSB_INT] = {
+		.pin	= 18,
+		.mode	= GPIO_FUNC_SIO,
+		.dir	= GPIO_IN,
+	},
+	[FUSB_VBUS] = {
+		.pin	= 26,
+		.mode	= GPIO_FUNC_SIO,
+		.dir	= GPIO_IN,
+	},
+	[UART_TX] = {		/* UART0 */
+		.pin	= 12,
+		.mode	= GPIO_FUNC_UART,
+	},
+	[UART_RX] = {		/* UART0 */
+		.pin	= 13,
+		.mode	= GPIO_FUNC_UART,
+	},
+};
+
+static const struct gpio_pin_config m1_pd_bmc_pin_config1[] = {
+	[LED_G] = {
+		.pin	= 25,
+		.mode	= GPIO_FUNC_SIO,
+		.dir	= GPIO_OUT,
+		.skip	= true,
+	},
+	[I2C_SDA] = {		/* I2C1 */
+		.pin	= 22,
+		.mode	= GPIO_FUNC_I2C,
+	},
+	[I2C_SCL] = {		/* I2C1 */
+		.pin	= 27,
+		.mode	= GPIO_FUNC_I2C,
+	},
+	[FUSB_INT] = {
+		.pin	= 19,
+		.mode	= GPIO_FUNC_SIO,
+		.dir	= GPIO_IN,
+	},
+	[FUSB_VBUS] = {
+		.pin	= 28,
+		.mode	= GPIO_FUNC_SIO,
+		.dir	= GPIO_IN,
+	},
+	[UART_TX] = {		/* UART1 */
+		.pin	= 8,
+		.mode	= GPIO_FUNC_UART,
+	},
+	[UART_RX] = {		/* UART1 */
+		.pin	= 9,
+		.mode	= GPIO_FUNC_UART,
+	},
+};
+
+#define ARRAY_SIZE(arr)	(sizeof(arr) / sizeof((arr)[0]))
+
+static void __not_in_flash_func(uart_irq_fn)(const struct hw_context *hw)
+{
+	while (uart_is_readable(hw->uart))
+		putchar_raw(uart_getc(hw->uart));
+}
+
+static void uart0_irq_fn(void);
+static void uart1_irq_fn(void);
+
+static const struct hw_context hw0 = {
+	.pins		= m1_pd_bmc_pin_config0,
+	.nr_pins	= ARRAY_SIZE(m1_pd_bmc_pin_config0),
+	.uart		= uart0,
+	.uart_irq	= UART0_IRQ,
+	.uart_handler	= uart0_irq_fn,
+	.i2c		= i2c0,
+	.addr		= fusb302_I2C_SLAVE_ADDR,
+};
+
+static const struct hw_context hw1 = {
+	.pins		= m1_pd_bmc_pin_config1,
+	.nr_pins	= ARRAY_SIZE(m1_pd_bmc_pin_config1),
+	.uart		= uart1,
+	.uart_irq	= UART1_IRQ,
+	.uart_handler	= uart1_irq_fn,
+	.i2c		= i2c1,
+	.addr		= fusb302_I2C_SLAVE_ADDR,
+};
+
+static void __not_in_flash_func(uart0_irq_fn)(void)
+{
+	uart_irq_fn(&hw0);
+}
+
+static void __not_in_flash_func(uart1_irq_fn)(void)
+{
+	uart_irq_fn(&hw1);
+}
+
+static void init_system(const struct hw_context *hw)
+{
+	i2c_init(hw->i2c, 400 * 1000);
+
+	uart_init(hw->uart, 115200);
+	uart_set_hw_flow(hw->uart, false, false);
+	uart_set_fifo_enabled(hw->uart, true);
+	irq_set_exclusive_handler(hw->uart_irq, hw->uart_handler);
+	irq_set_enabled(hw->uart_irq, true);
+	uart_set_irq_enables(hw->uart, true, false);
+
+	/* Interrupt when the RX FIFO is 3/4 full */
+	hw_write_masked(&uart_get_hw(hw->uart)->ifls,
+			3 << UART_UARTIFLS_RXIFLSEL_LSB,
+                        UART_UARTIFLS_RXIFLSEL_BITS);
+}
+
+static void m1_pd_bmc_gpio_setup_one(const struct gpio_pin_config *pin)
+{
+	if (pin->skip)
+		return;
+	gpio_set_function(pin->pin, pin->mode);
+	if (pin->mode == GPIO_FUNC_SIO) {
+		gpio_init(pin->pin);
+		gpio_set_dir(pin->pin, pin->dir);
+	}
+	if (pin->pu)
+		gpio_pull_up(pin->pin);
+}
+
+static void m1_pd_bmc_system_init(const struct hw_context *hw)
+{
+	init_system(hw);
+
+	for (unsigned int i = 0; i < hw->nr_pins; i++)
+		m1_pd_bmc_gpio_setup_one(&hw->pins[i]);
+}
+
+int main(void)
+{
+	bool success;
+
+	success = set_sys_clock_khz(250000, false);
+
+	stdio_init_all();
+
+	m1_pd_bmc_system_init(&hw0);
+	m1_pd_bmc_system_init(&hw1);
+
+	while (!stdio_usb_connected()) {
+		static bool state = false;
+
+		gpio_put(hw0.pins[LED_G].pin, state);
+		sleep_ms(250);
+		state = !state;
+	}
+
+	printf("This is the Central Scrutinizer\n"
+		"Control character is ^_\n"
+		"Press ^_ + ? for help\n");
+	if (!success)
+		printf("WARNING: Nominal frequency NOT reached\n");
+
+	m1_pd_bmc_fusb_setup(0, &hw0);
+	m1_pd_bmc_fusb_setup(1, &hw1);
+
+	m1_pd_bmc_run();
+}
diff --git a/tcpm.h b/tcpm.h
new file mode 100644
index 0000000..862dfff
--- /dev/null
+++ b/tcpm.h
@@ -0,0 +1,43 @@
+/* Copyright 2015 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 - common header for TCPM drivers */
+
+#ifndef __CROS_EC_USB_PD_TCPM_TCPM_H
+#define __CROS_EC_USB_PD_TCPM_TCPM_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "tcpm_driver.h"
+#include "usb_pd_tcpm.h"
+
+#if defined(CONFIG_USB_PD_DUAL_ROLE_AUTO_TOGGLE) && \
+	!defined(CONFIG_USB_PD_DUAL_ROLE)
+#error "DRP auto toggle requires board to have DRP support"
+#error "Please upgrade your board configuration"
+#endif
+
+#ifndef CONFIG_USB_PD_TCPC
+extern const struct tcpc_config_t tcpc_config[];
+
+/* I2C wrapper functions - get I2C port / slave addr from config struct. */
+int16_t tcpc_write(int16_t port, int16_t reg, int16_t val);
+int16_t tcpc_write16(int16_t port, int16_t reg, int16_t val);
+int16_t tcpc_read(int16_t port, int16_t reg, int16_t *val);
+int16_t tcpc_read16(int16_t port, int16_t reg, int16_t *val);
+int16_t tcpc_xfer(int16_t port,
+		const uint8_t *out, int16_t out_size,
+		uint8_t *in, int16_t in_size,
+		int16_t flags);
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/tcpm_driver.c b/tcpm_driver.c
new file mode 100644
index 0000000..a6eda48
--- /dev/null
+++ b/tcpm_driver.c
@@ -0,0 +1,85 @@
+#include <stdio.h>
+
+#include "m1-pd-bmc.h"
+#include "tcpm_driver.h"
+
+/* I2C wrapper functions - get I2C port / slave addr from config struct. */
+int16_t tcpc_write(int16_t port, int16_t reg, int16_t val)
+{
+	const struct hw_context *fusb = get_hw_from_port(port);
+	uint8_t buf[] = {
+		reg & 0xff,
+		val & 0xff,
+	};
+
+	i2c_write_blocking(fusb->i2c, fusb->addr, buf, sizeof(buf), false);
+
+	return 0;
+}
+
+int16_t tcpc_write16(int16_t port, int16_t reg, int16_t val)
+{
+	const struct hw_context *fusb = get_hw_from_port(port);
+	uint8_t buf[] = {
+		reg & 0xff,
+		val & 0xff,
+		(val >> 8) & 0xff,
+	};
+
+	i2c_write_blocking(fusb->i2c, fusb->addr, buf, sizeof(buf), false);
+
+	return 0;
+}
+
+int16_t tcpc_read(int16_t port, int16_t reg, int16_t *val)
+{
+	const struct hw_context *fusb = get_hw_from_port(port);
+	uint8_t buf[] = {
+		reg & 0xff,
+		0,
+	};
+
+	i2c_write_blocking(fusb->i2c, fusb->addr, &buf[0], 1, true);
+	i2c_read_blocking(fusb->i2c, fusb->addr, &buf[1], 1, false);
+
+	*val = buf[1];
+
+	return 0;
+}
+
+int16_t tcpc_read16(int16_t port, int16_t reg, int16_t *val)
+{
+	const struct hw_context *fusb = get_hw_from_port(port);
+	uint8_t buf[] = {
+		reg & 0xff,
+		0,
+		0,
+	};
+
+	i2c_write_blocking(fusb->i2c, fusb->addr, &buf[0], 1, true);
+	i2c_read_blocking(fusb->i2c, fusb->addr, &buf[1], 2, false);
+	*val = buf[1];
+	*val |= (buf[2] << 8);
+
+	return 0;
+}
+
+int16_t tcpc_xfer(int16_t port,
+	      const uint8_t * out, int16_t out_size,
+	      uint8_t * in, int16_t in_size, int16_t flags)
+{
+	const struct hw_context *fusb = get_hw_from_port(port);
+
+	if (out_size) {
+		int err;
+		err = i2c_write_blocking(fusb->i2c, fusb->addr, out, out_size,
+					 !(flags & I2C_XFER_STOP));
+	}
+
+	if (in_size) {
+		i2c_read_blocking(fusb->i2c, fusb->addr, in, in_size,
+				  !(flags & I2C_XFER_STOP));
+	}
+
+	return 0;
+}
diff --git a/tcpm_driver.h b/tcpm_driver.h
new file mode 100644
index 0000000..e20deea
--- /dev/null
+++ b/tcpm_driver.h
@@ -0,0 +1,28 @@
+/*
+ * tcpm_driver.h
+ *
+ * Created: 11/11/2017 18:42:39
+ *  Author: jason
+ */ 
+
+
+#ifndef TCPM_DRIVER_H_
+#define TCPM_DRIVER_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+// USB-C Stuff
+#include "tcpm.h"
+#include "FUSB302.h"
+#define CONFIG_USB_PD_PORT_COUNT 2
+extern struct i2c_master_module i2c_master_instance;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* TCPM_DRIVER_H_ */
diff --git a/usb_pd_tcpm.h b/usb_pd_tcpm.h
new file mode 100644
index 0000000..51c5c47
--- /dev/null
+++ b/usb_pd_tcpm.h
@@ -0,0 +1,202 @@
+/* Copyright 2015 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 */
+
+#ifndef __CROS_EC_USB_PD_TCPM_H
+#define __CROS_EC_USB_PD_TCPM_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* List of common error codes that can be returned */
+	enum ec_error_list {
+		/* Success - no error */
+		EC_SUCCESS = 0,
+		/* Unknown error */
+		EC_ERROR_UNKNOWN = 1,
+		/* Function not implemented yet */
+		EC_ERROR_UNIMPLEMENTED = 2,
+		/* Overflow error; too much input provided. */
+		EC_ERROR_OVERFLOW = 3,
+		/* Timeout */
+		EC_ERROR_TIMEOUT = 4,
+		/* Invalid argument */
+		EC_ERROR_INVAL = 5,
+		/* Already in use, or not ready yet */
+		EC_ERROR_BUSY = 6,
+		/* Access denied */
+		EC_ERROR_ACCESS_DENIED = 7,
+		/* Failed because component does not have power */
+		EC_ERROR_NOT_POWERED = 8,
+		/* Failed because component is not calibrated */
+		EC_ERROR_NOT_CALIBRATED = 9,
+		/* Failed because CRC error */
+		EC_ERROR_CRC = 10,
+	};
+
+/* Flags for i2c_xfer() */
+#define I2C_XFER_START (1 << 0)	/* Start smbus session from idle state */
+#define I2C_XFER_STOP (1 << 1)	/* Terminate smbus session with stop bit */
+#define I2C_XFER_SINGLE (I2C_XFER_START | I2C_XFER_STOP)	/* One transaction */
+
+/* Default retry count for transmitting */
+#define PD_RETRY_COUNT 3
+
+/* Time to wait for TCPC to complete transmit */
+#define PD_T_TCPC_TX_TIMEOUT  (100*MSEC_US)
+
+/* No connect voltage threshold for sources based on Rp */
+#define PD_SRC_DEF_VNC_MV        1600
+#define PD_SRC_1_5_VNC_MV        1600
+#define PD_SRC_3_0_VNC_MV        2600
+
+/* Rd voltage threshold for sources based on Rp */
+#define PD_SRC_DEF_RD_THRESH_MV  200
+#define PD_SRC_1_5_RD_THRESH_MV  400
+#define PD_SRC_3_0_RD_THRESH_MV  800
+
+/* Control Message type */
+	enum pd_ctrl_msg_type {
+		/* 0 Reserved */
+		PD_CTRL_GOOD_CRC = 1,
+		PD_CTRL_GOTO_MIN = 2,
+		PD_CTRL_ACCEPT = 3,
+		PD_CTRL_REJECT = 4,
+		PD_CTRL_PING = 5,
+		PD_CTRL_PS_RDY = 6,
+		PD_CTRL_GET_SOURCE_CAP = 7,
+		PD_CTRL_GET_SINK_CAP = 8,
+		PD_CTRL_DR_SWAP = 9,
+		PD_CTRL_PR_SWAP = 10,
+		PD_CTRL_VCONN_SWAP = 11,
+		PD_CTRL_WAIT = 12,
+		PD_CTRL_SOFT_RESET = 13,
+		/* 14-15 Reserved */
+
+		/* Used for REV 3.0 */
+		PD_CTRL_NOT_SUPPORTED = 16,
+		PD_CTRL_GET_SOURCE_CAP_EXT = 17,
+		PD_CTRL_GET_STATUS = 18,
+		PD_CTRL_FR_SWAP = 19,
+		PD_CTRL_GET_PPS_STATUS = 20,
+		PD_CTRL_GET_COUNTRY_CODES = 21,
+		/* 22-31 Reserved */
+	};
+
+/* Extended message type for REV 3.0 */
+	enum pd_ext_msg_type {
+		/* 0 Reserved */
+		PD_EXT_SOURCE_CAP = 1,
+		PD_EXT_STATUS = 2,
+		PD_EXT_GET_BATTERY_CAP = 3,
+		PD_EXT_GET_BATTERY_STATUS = 4,
+		PD_EXT_BATTERY_CAP = 5,
+		PD_EXT_GET_MANUFACTURER_INFO = 6,
+		PD_EXT_MANUFACTURER_INFO = 7,
+		PD_EXT_SECURITY_REQUEST = 8,
+		PD_EXT_SECURITY_RESPONSE = 9,
+		PD_EXT_FIRMWARE_UPDATE_REQUEST = 10,
+		PD_EXT_FIRMWARE_UPDATE_RESPONSE = 11,
+		PD_EXT_PPS_STATUS = 12,
+		PD_EXT_COUNTRY_INFO = 13,
+		PD_EXT_COUNTRY_CODES = 14,
+		/* 15-31 Reserved */
+	};
+
+/* Data message type */
+	enum pd_data_msg_type {
+		/* 0 Reserved */
+		PD_DATA_SOURCE_CAP = 1,
+		PD_DATA_REQUEST = 2,
+		PD_DATA_BIST = 3,
+		PD_DATA_SINK_CAP = 4,
+		/* 5-14 Reserved for REV 2.0 */
+		PD_DATA_BATTERY_STATUS = 5,
+		PD_DATA_ALERT = 6,
+		PD_DATA_GET_COUNTRY_INFO = 7,
+		/* 8-14 Reserved for REV 3.0 */
+		PD_DATA_VENDOR_DEF = 15,
+	};
+/* build extended message header */
+/* All extended messages are chunked, so set bit 15 */
+#define PD_EXT_HEADER(cnum, rchk, dsize) \
+   ((1 << 15) | ((cnum) << 11) | \
+   ((rchk) << 10) | (dsize))
+
+/* build message header */
+#define PD_HEADER(type, prole, drole, id, cnt, rev, ext) \
+  ((type) | ((rev) << 6) | \
+  ((drole) << 5) | ((prole) << 8) | \
+  ((id) << 9) | ((cnt) << 12) | ((ext) << 15))
+
+/* Used for processing pd header */
+#define PD_HEADER_EXT(header)  (((header) >> 15) & 1)
+#define PD_HEADER_CNT(header)  (((header) >> 12) & 7)
+#define PD_HEADER_TYPE(header) ((header) & 0x1F)
+#define PD_HEADER_ID(header)   (((header) >> 9) & 7)
+#define PD_HEADER_REV(header)  (((header) >> 6) & 3)
+
+/* Used for processing pd extended header */
+#define PD_EXT_HEADER_CHUNKED(header)   (((header) >> 15) & 1)
+#define PD_EXT_HEADER_CHUNK_NUM(header) (((header) >> 11) & 0xf)
+#define PD_EXT_HEADER_REQ_CHUNK(header) (((header) >> 10) & 1)
+#define PD_EXT_HEADER_DATA_SIZE(header) ((header) & 0x1ff)
+
+#define PD_REV10 0
+#define PD_REV20 1
+#define PD_REV30 2
+
+	enum tcpc_cc_voltage_status {
+		TYPEC_CC_VOLT_OPEN = 0,
+		TYPEC_CC_VOLT_RA = 1,
+		TYPEC_CC_VOLT_RD = 2,
+		TYPEC_CC_VOLT_SNK_DEF = 5,
+		TYPEC_CC_VOLT_SNK_1_5 = 6,
+		TYPEC_CC_VOLT_SNK_3_0 = 7,
+	};
+
+	enum tcpc_cc_pull {
+		TYPEC_CC_RA = 0,
+		TYPEC_CC_RP = 1,
+		TYPEC_CC_RD = 2,
+		TYPEC_CC_OPEN = 3,
+	};
+
+	enum tcpc_rp_value {
+		TYPEC_RP_USB = 0,
+		TYPEC_RP_1A5 = 1,
+		TYPEC_RP_3A0 = 2,
+		TYPEC_RP_RESERVED = 3,
+	};
+
+	enum tcpm_transmit_type {
+		TCPC_TX_SOP = 0,
+		TCPC_TX_SOP_PRIME = 1,
+		TCPC_TX_SOP_PRIME_PRIME = 2,
+		TCPC_TX_SOP_DEBUG_PRIME = 3,
+		TCPC_TX_SOP_DEBUG_PRIME_PRIME = 4,
+		TCPC_TX_HARD_RESET = 5,
+		TCPC_TX_CABLE_RESET = 6,
+		TCPC_TX_BIST_MODE_2 = 7
+	};
+
+	enum tcpc_transmit_complete {
+		TCPC_TX_COMPLETE_SUCCESS = 0,
+		TCPC_TX_COMPLETE_DISCARDED = 1,
+		TCPC_TX_COMPLETE_FAILED = 2,
+	};
+
+	struct tcpc_config_t {
+		int i2c_host_port;
+		int i2c_slave_addr;
+		const struct tcpm_drv *drv;
+	};
+
+#ifdef __cplusplus
+}
+#endif
+#endif				/* __CROS_EC_USB_PD_TCPM_H */
diff --git a/vdmtool.c b/vdmtool.c
new file mode 100644
index 0000000..4f7916d
--- /dev/null
+++ b/vdmtool.c
@@ -0,0 +1,659 @@
+#include <stdint.h>
+#include <stddef.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "tcpm_driver.h"
+#include "FUSB302.h"
+#include "m1-pd-bmc.h"
+#include "hardware/watchdog.h"
+#include "hardware/sync.h"
+#include "pico/bootrom.h"
+
+enum state {
+	STATE_INVALID = -1,
+	STATE_DISCONNECTED = 0,
+	STATE_CONNECTED,
+	STATE_READY,
+	STATE_DFP_VBUS_ON,
+	STATE_DFP_CONNECTED,
+	STATE_DFP_ACCEPT,
+	STATE_IDLE,
+};
+
+struct vdm_context {
+	const struct hw_context		*hw;
+	enum state			state;
+	int16_t				std_flag;
+	int16_t				source_cap_timer;
+	int16_t				cc_debounce;
+	volatile bool			pending;
+	bool 				verbose;
+	bool				vdm_escape;
+};
+
+static struct vdm_context vdm_contexts[CONFIG_USB_PD_PORT_COUNT];
+
+#define PIN(cxt, idx)	(cxt)->hw->pins[(idx)].pin
+#define PORT(cxt)	((cxt) - vdm_contexts)
+#define UART(cxt)	(cxt)->hw->uart
+
+#define HIGH true
+#define LOW false
+
+#define dprintf(cxt, ...)	do {					\
+		if (cxt->verbose)					\
+			printf(__VA_ARGS__);				\
+		} while(0)
+
+#define cprintf(cxt, str, ...)	do {					\
+		printf("P%ld: " str, PORT(cxt), ##__VA_ARGS__);		\
+	} while(0)
+
+#define STATE(cxt, x)	do {						\
+		cxt->state = STATE_##x;					\
+		cprintf(cxt, "S: " #x "\n");				\
+	} while(0)
+
+static void vbus_off(struct vdm_context *cxt)
+{
+	gpio_put(PIN(cxt, FUSB_VBUS), LOW);
+	sleep_ms(800);
+	gpio_set_dir(PIN(cxt, FUSB_VBUS), GPIO_IN);
+	cprintf(cxt, "VBUS OFF\n");
+}
+
+static void vbus_on(struct vdm_context *cxt)
+{
+	cprintf(cxt, "VBUS ON\n");
+	gpio_set_dir(PIN(cxt, FUSB_VBUS), GPIO_OUT);
+	gpio_put(PIN(cxt, FUSB_VBUS), HIGH);
+}
+
+void debug_poke(struct vdm_context *cxt)
+{
+	int16_t hdr = PD_HEADER(PD_DATA_VENDOR_DEF, 1, 1, 0, 1, PD_REV20, 0);
+	const uint32_t x = 0;
+
+	cprintf(cxt, "Empty debug message\n");
+	fusb302_tcpm_transmit(PORT(cxt), TCPC_TX_SOP_DEBUG_PRIME_PRIME, hdr, &x);
+}
+
+static void evt_dfpconnect(struct vdm_context *cxt)
+{
+	int16_t cc1 = -1, cc2 = -1;
+	fusb302_tcpm_get_cc(PORT(cxt), &cc1, &cc2);
+	cprintf(cxt, "Connected: cc1=%d cc2=%d\n", cc1, cc2);
+	if (cc1 < 2 && cc2 < 2) {
+		cprintf(cxt, "Nope.\n");
+		return;
+	}
+	fusb302_pd_reset(PORT(cxt));
+	fusb302_tcpm_set_msg_header(PORT(cxt), 1, 1);	// Source
+	if (cc1 > cc2) {
+		fusb302_tcpm_set_polarity(PORT(cxt), 0);
+		cprintf(cxt, "Polarity: CC1 (normal)\n");
+	} else {
+		fusb302_tcpm_set_polarity(PORT(cxt), 1);
+		cprintf(cxt, "Polarity: CC2 (flipped)\n");
+	}
+	fusb302_tcpm_set_rx_enable(PORT(cxt), 1);
+	vbus_on(cxt);
+	STATE(cxt, DFP_VBUS_ON);
+
+	debug_poke(cxt);
+}
+
+static void evt_connect(struct vdm_context *cxt)
+{
+	int16_t cc1 = -1, cc2 = -1;
+	fusb302_tcpm_get_cc(PORT(cxt), &cc1, &cc2);
+	cprintf(cxt, "Connected: cc1=%d cc2=%d\n", cc1, cc2);
+	if (cc1 < 2 && cc2 < 2) {
+		cprintf(cxt, "Nope.\n");
+		return;
+	}
+	fusb302_pd_reset(PORT(cxt));
+	fusb302_tcpm_set_msg_header(PORT(cxt), 0, 0);	// Sink
+	if (cc1 > cc2) {
+		fusb302_tcpm_set_polarity(PORT(cxt), 0);
+		cprintf(cxt, "Polarity: CC1 (normal)\n");
+	} else {
+		fusb302_tcpm_set_polarity(PORT(cxt), 1);
+		cprintf(cxt, "Polarity: CC2 (flipped)\n");
+	}
+	fusb302_tcpm_set_rx_enable(PORT(cxt), 1);
+	STATE(cxt, CONNECTED);
+}
+
+static void evt_disconnect(struct vdm_context *cxt)
+{
+	vbus_off(cxt);
+	cprintf(cxt, "Disconnected\n");
+	fusb302_pd_reset(PORT(cxt));
+	fusb302_tcpm_set_rx_enable(PORT(cxt), 0);
+	fusb302_tcpm_select_rp_value(PORT(cxt), TYPEC_RP_USB);
+	fusb302_tcpm_set_cc(PORT(cxt), TYPEC_CC_RP);	// DFP mode
+	STATE(cxt, DISCONNECTED);
+}
+
+static void send_power_request(struct vdm_context *cxt, uint32_t cap)
+{
+	int16_t hdr = PD_HEADER(PD_DATA_REQUEST, 0, 0, 0, 1, PD_REV20, 0);
+	uint32_t req =
+		(1L << 28) | // Object position (fixed 5V)
+		(1L << 25) | // USB communications capable
+		(0L << 10) | // 0mA operating
+		(0L << 0);   // 0mA max
+
+	fusb302_tcpm_transmit(PORT(cxt), TCPC_TX_SOP, hdr, &req);
+	cprintf(cxt, ">REQUEST\n");
+	(void)cap;
+}
+
+static void send_sink_cap(struct vdm_context *cxt)
+{
+	int16_t hdr = PD_HEADER(PD_DATA_SINK_CAP, 1, 1, 0, 1, PD_REV20, 0);
+	uint32_t cap =
+		(1L << 26) | // USB communications capable
+		(0L << 10) | // 0mA operating
+		(0L << 0);   // 0mA max
+
+	fusb302_tcpm_transmit(PORT(cxt), TCPC_TX_SOP, hdr, &cap);
+	cprintf(cxt, ">SINK_CAP\n");
+	STATE(cxt, READY);
+}
+
+static void send_source_cap(struct vdm_context *cxt)
+{
+	int16_t hdr = PD_HEADER(PD_DATA_SOURCE_CAP, 1, 1, 0, 1, PD_REV20, 0);
+	uint32_t cap = 0x37019096;
+
+	fusb302_tcpm_transmit(PORT(cxt), TCPC_TX_SOP, hdr, &cap);
+	cprintf(cxt, ">SOURCE_CAP\n");
+	cxt->source_cap_timer = 0;
+}
+
+static void dump_msg(enum fusb302_rxfifo_tokens sop, int16_t hdr, uint32_t * msg)
+{
+	int16_t len = PD_HEADER_CNT(hdr);
+	switch (sop) {
+	case fusb302_TKN_SOP:
+		printf("RX SOP (");
+		break;
+	case fusb302_TKN_SOP1:
+		printf("RX SOP' (");
+		break;
+	case fusb302_TKN_SOP2:
+		printf("RX SOP\" (");
+		break;
+	case fusb302_TKN_SOP1DB:
+		printf("RX SOP'DEBUG (");
+		break;
+	case fusb302_TKN_SOP2DB:
+		printf("RX SOP\"DEBUG (");
+		break;
+	default:
+		printf("RX ? (");
+		break;
+	}
+
+	printf("%d) [%x]", len, hdr);
+	for (int16_t i = 0; i < PD_HEADER_CNT(hdr); i++) {
+		printf(" ");
+		printf("%x", msg[i]);
+	}
+	printf("\n");
+}
+
+static void handle_discover_identity(struct vdm_context *cxt)
+{
+	int16_t hdr = PD_HEADER(PD_DATA_VENDOR_DEF, 0, 0, 0, 4, PD_REV20, 0);
+	uint32_t vdm[4] = {
+		0xff008001L | (1L << 6),	// ACK
+
+		(1L << 30) |	// USB Device
+		(0L << 27) |	// UFP Product Type = Undefined
+		(0L << 26) |	// No modal operation
+		(0L << 23) |	// DFP Product Type = Undefined
+		0x5acL,	// USB VID = Apple
+
+		0L,		// XID
+
+		(0x0001L << 16) |	// USB PID,
+		0x100L	// bcdDevice
+	};
+
+	fusb302_tcpm_transmit(PORT(cxt), TCPC_TX_SOP, hdr, vdm);
+	cprintf(cxt, ">VDM DISCOVER_IDENTITY\n");
+}
+
+static void handle_power_request(struct vdm_context *cxt, uint32_t req)
+{
+	int16_t hdr = PD_HEADER(PD_CTRL_ACCEPT, 1, 1, 0, 0, PD_REV20, 0);
+
+	fusb302_tcpm_transmit(PORT(cxt), TCPC_TX_SOP, hdr, NULL);
+	cprintf(cxt, ">ACCEPT\n");
+	STATE(cxt, DFP_ACCEPT);
+}
+
+static void send_ps_rdy(struct vdm_context *cxt)
+{
+	int16_t hdr = PD_HEADER(PD_CTRL_PS_RDY, 1, 1, 0, 0, PD_REV20, 0);
+	fusb302_tcpm_transmit(PORT(cxt), TCPC_TX_SOP, hdr, NULL);
+	cprintf(cxt, ">PS_RDY\n");
+
+	STATE(cxt, IDLE);
+}
+
+static void send_reject(struct vdm_context *cxt)
+{
+	int16_t hdr = PD_HEADER(PD_CTRL_REJECT, 1, 1, 0, 0, PD_REV20, 0);
+
+	fusb302_tcpm_transmit(PORT(cxt), TCPC_TX_SOP, hdr, NULL);
+	cprintf(cxt, ">REJECT\n");
+
+	STATE(cxt, IDLE);
+}
+
+static void handle_vdm(struct vdm_context *cxt, enum fusb302_rxfifo_tokens sop,
+		       int16_t hdr, uint32_t *msg)
+{
+	switch (*msg) {
+	case 0xff008001:	// Structured VDM: DISCOVER IDENTITY
+		cprintf(cxt, "<VDM DISCOVER_IDENTITY\n");
+		handle_discover_identity(cxt);
+		STATE(cxt, READY);
+		break;
+	default:
+		cprintf(cxt, "<VDM ");
+		dump_msg(sop, hdr, msg);
+		break;
+	}
+}
+
+static void handle_msg(struct vdm_context *cxt, enum fusb302_rxfifo_tokens sop,
+		       int16_t hdr, uint32_t *msg)
+{
+	int16_t len = PD_HEADER_CNT(hdr);
+	int16_t type = PD_HEADER_TYPE(hdr);
+
+	if (len != 0) {
+		switch (type) {
+		case PD_DATA_SOURCE_CAP:
+			cprintf(cxt, "<SOURCE_CAP: %x\n", msg[0]);
+			send_power_request(cxt, msg[0]);
+			break;
+		case PD_DATA_REQUEST:
+			cprintf(cxt, "<REQUEST: %x\n", msg[0]);
+			handle_power_request(cxt, msg[0]);
+			break;
+		case PD_DATA_VENDOR_DEF:
+			handle_vdm(cxt, sop, hdr, msg);
+			break;
+		default:
+			cprintf(cxt, "<UNK DATA ");
+			dump_msg(sop, hdr, msg);
+			break;
+		}
+	} else {
+		switch (type) {
+		case PD_CTRL_ACCEPT:
+			cprintf(cxt, "<ACCEPT\n");
+			break;
+		case PD_CTRL_REJECT:
+			cprintf(cxt, "<REJECT\n");
+			break;
+		case PD_CTRL_PS_RDY:
+			cprintf(cxt, "<PS_RDY\n");
+			break;
+		case PD_CTRL_PR_SWAP:
+			cprintf(cxt, "<PR_SWAP\n");
+			send_reject(cxt);
+			break;
+		case PD_CTRL_DR_SWAP:
+			cprintf(cxt, "<DR_SWAP\n");
+			send_reject(cxt);
+			break;
+		case PD_CTRL_GET_SINK_CAP:
+			cprintf(cxt, "<GET_SINK_CAP\n");
+			send_sink_cap(cxt);
+			break;
+		default:
+			cprintf(cxt, "<UNK CTL ");
+			dump_msg(sop, hdr, msg);
+			break;
+		}
+	}
+}
+
+static void evt_packet(struct vdm_context *cxt)
+{
+	int16_t hdr, len, ret;
+	enum fusb302_rxfifo_tokens sop;
+	uint32_t msg[16];
+
+	ret = fusb302_tcpm_get_message(PORT(cxt), msg, &hdr, &sop);
+	if (ret) {
+		// No packet or GoodCRC
+		return;
+	}
+
+	handle_msg(cxt, sop, hdr, msg);
+}
+
+static void vdm_fun(struct vdm_context *cxt);
+
+static void evt_sent(struct vdm_context *cxt)
+{
+	switch (cxt->state) {
+	case STATE_DFP_VBUS_ON:
+		STATE(cxt, DFP_CONNECTED);
+		vdm_fun(cxt);
+		break;
+	case STATE_DFP_ACCEPT:
+		send_ps_rdy(cxt);
+		break;
+	default:
+		break;
+	}
+}
+
+static void handle_irq(struct vdm_context *cxt)
+{
+	int16_t irq, irqa, irqb;
+	fusb302_get_irq(PORT(cxt), &irq, &irqa, &irqb);
+
+	cprintf(cxt, "IRQ=%x %x %x\n", irq, irqa, irqb);
+	if (irq & TCPC_REG_INTERRUPT_VBUSOK) {
+		cprintf(cxt, "IRQ: VBUSOK (VBUS=");
+		if (fusb302_tcpm_get_vbus_level(PORT(cxt))) {
+			cprintf(cxt, "ON)\n");
+			send_source_cap(cxt);
+			debug_poke(cxt);
+		} else {
+			cprintf(cxt, "OFF)\n");
+			evt_disconnect(cxt);
+		}
+	}
+	if (irqa & TCPC_REG_INTERRUPTA_HARDRESET) {
+		cprintf(cxt, "IRQ: HARDRESET\n");
+		evt_disconnect(cxt);
+	}
+	if (irqa & TCPC_REG_INTERRUPTA_TX_SUCCESS) {
+		//cprintf(cxt, "IRQ: TXSUCCESS\n");
+		evt_sent(cxt);
+	}
+	if (irqb & TCPC_REG_INTERRUPTB_GCRCSENT) {
+		//cprintf(cxt, "IRQ: GCRCSENT\n");
+		while (!fusb302_rx_fifo_is_empty(PORT(cxt)))
+			evt_packet(cxt);
+	}
+}
+
+static void vdm_fun(struct vdm_context *cxt)
+{
+
+	//uint32_t vdm[] = { 0x5ac8010 }; // Get Action List
+	//uint32_t vdm[] = { 0x5ac8012, 0x0105, 0x8002<<16 }; // PMU Reset + DFU Hold
+	//uint32_t vdm[] = { 0x5ac8011, 0x0809  }; // Get Action List
+	//uint32_t vdm[] = { 0x5ac8012, 0x0105, 0x8000<<16 };
+
+	// VDM to mux debug UART over SBU1/2
+	uint32_t vdm[] = { 0x5AC8012, 0x01840306 };
+
+	int16_t hdr = PD_HEADER(PD_DATA_VENDOR_DEF, 1, 1, 0, sizeof(vdm) / 4, PD_REV20, 0);
+	fusb302_tcpm_transmit(PORT(cxt), TCPC_TX_SOP_DEBUG_PRIME_PRIME, hdr, vdm);
+	cprintf(cxt, ">VDM serial -> SBU1/2\n");
+
+}
+
+void vdm_send_reboot(struct vdm_context *cxt)
+{
+	uint32_t vdm[] = { 0x5ac8012, 0x0105, 0x8000UL<<16 };
+	int hdr = PD_HEADER(PD_DATA_VENDOR_DEF, 1, 1, 0, sizeof(vdm) / 4, PD_REV20, 0);
+	fusb302_tcpm_transmit(PORT(cxt), TCPC_TX_SOP_DEBUG_PRIME_PRIME, hdr, vdm);
+	cprintf(cxt, ">VDM SET ACTION reboot\n\r");
+}
+
+static void serial_out(struct vdm_context *cxt, char c)
+{
+	uart_putc_raw(UART(cxt), c);
+}
+
+static void help(void)
+{
+	printf("^_    Escape character\n"
+	       "^_ ^_ Raw ^_\n"
+	       "^_ ^@ Send break\n"
+	       "^_ !  DUT reset\n"
+	       "^_ ^R Central Scrutinizer reset\n"
+	       "^_ ^^ Central Scrutinizer reset to programming mode\n"
+	       "^_ ^D Toggle debug\n"
+	       "^_ ^M Send empty debug VDM\n"
+	       "^_ ?  This message\n");
+	for (int i = 0; i < CONFIG_USB_PD_PORT_COUNT; i++)
+		cprintf(&vdm_contexts[i], "%s\n",
+			vdm_contexts[i].hw ? "present" : "absent");
+}
+
+static bool serial_handler(struct vdm_context *cxt)
+{
+	bool uart_active = false;
+	int c;
+
+	while ((c = getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) {
+		uart_active = true;
+
+		if ((!cxt->vdm_escape && c != 0x1f)) {
+			serial_out(cxt, c);
+			continue;
+		}
+
+		switch (c) {
+		case '!':			/* ! */
+			vdm_send_reboot(cxt);
+			break;
+		case 0x12:			/* ^R */
+			watchdog_enable(1, 1);
+			break;
+		case 0x1E:			/* ^^ */
+			reset_usb_boot(1 << PICO_DEFAULT_LED_PIN,0);
+			break;
+		case 0x1F:			/* ^_ */
+			if (!cxt->vdm_escape) {
+				cxt->vdm_escape = true;
+				continue;
+			}
+
+			serial_out(cxt, c);
+			break;
+		case 4:				/* ^D */
+			cxt->verbose = !cxt->verbose;
+			break;
+		case 0:				/* ^@ */
+			uart_set_break(UART(cxt), true);
+			sleep_ms(1);
+			uart_set_break(UART(cxt), false);
+			break;
+		case '\r':			/* Enter */
+			debug_poke(cxt);
+			break;
+		case '?':
+			help();
+			break;
+		}
+
+		cxt->vdm_escape = false;
+	}
+
+	return uart_active;
+}
+
+static void state_machine(struct vdm_context *cxt)
+{
+	switch (cxt->state) {
+	case STATE_DISCONNECTED:{
+		int16_t cc1 = -1, cc2 = -1;
+		fusb302_tcpm_get_cc(PORT(cxt), &cc1, &cc2);
+		dprintf(cxt, "Poll: cc1=%d  cc2=%d\n", (int)cc1, (int)cc2);
+		sleep_ms(200);
+		if (cc1 >= 2 || cc2 >= 2)
+			evt_dfpconnect(cxt);
+		break;
+	}
+	case STATE_CONNECTED:{
+		break;
+	}
+	case STATE_DFP_VBUS_ON:{
+		if (++cxt->source_cap_timer > 37) {
+			send_source_cap(cxt);
+			debug_poke(cxt);
+		}
+		break;
+	}
+	case STATE_DFP_CONNECTED:{
+		break;
+	}
+	case STATE_DFP_ACCEPT:{
+		break;
+	}
+	case STATE_READY:{
+		STATE(cxt, IDLE);
+		break;
+	}
+	case STATE_IDLE:{
+		break;
+	}
+	default:{
+		cprintf(cxt, "Invalid state %d", cxt->state);
+		cprintf(cxt, "\n");
+	}
+	}
+	if (cxt->state != STATE_DISCONNECTED) {
+		int16_t cc1 = -1, cc2 = -1;
+		fusb302_tcpm_get_cc(PORT(cxt), &cc1, &cc2);
+		if (cc1 < 2 && cc2 < 2) {
+			if (cxt->cc_debounce++ > 5) {
+				cprintf(cxt, "Disconnect: cc1=%d cc2=%d\n",
+					cc1, cc2);
+				evt_disconnect(cxt);
+				cxt->cc_debounce = 0;
+			}
+		} else {
+			cxt->cc_debounce = 0;
+		}
+	}
+}
+
+const struct hw_context *get_hw_from_port(int port)
+{
+	return vdm_contexts[port].hw;
+}
+
+static void fusb_int_handler(uint gpio, uint32_t event_mask)
+{
+	for (int i = 0; i < CONFIG_USB_PD_PORT_COUNT; i++) {
+		struct vdm_context *cxt = &vdm_contexts[i];
+
+		if (!cxt->hw)
+			continue;
+
+		if (gpio == PIN(cxt, FUSB_INT) &&
+		    (event_mask & GPIO_IRQ_LEVEL_LOW)) {
+			cxt->pending = true;
+			gpio_set_irq_enabled(PIN(cxt, FUSB_INT), GPIO_IRQ_LEVEL_LOW, false);
+		}
+	}
+}
+
+void m1_pd_bmc_fusb_setup(unsigned int port,
+			  const struct hw_context *hw)
+{
+	struct vdm_context *cxt;
+	int16_t reg;
+
+	if (port >= CONFIG_USB_PD_PORT_COUNT) {
+		cprintf(cxt, "Bad port number %d\n", port);
+		return;
+	}
+
+	cxt = vdm_contexts + port;
+	*cxt = (struct vdm_context) {
+		.hw			= hw,
+		.state 			= STATE_DISCONNECTED,
+		.source_cap_timer	= 0,
+		.cc_debounce		= 0,
+		.verbose		= false,
+		.vdm_escape		= false,
+	};
+
+	/*
+	 * If we can't infer pull-ups on the I2C, it is likely that
+	 * nothing is connected and we'd better skip this port.
+	 */
+	if (!gpio_get(PIN(cxt, I2C_SCL)) || !gpio_get(PIN(cxt, I2C_SDA))) {
+		cprintf(cxt, "I2C pins low while idling, skipping port\n");
+		cxt->hw = NULL;
+		return;
+	}
+
+	gpio_put(PIN(cxt, LED_G), HIGH);
+	vbus_off(cxt);
+
+	tcpc_read(PORT(cxt), TCPC_REG_DEVICE_ID, &reg);
+	cprintf(cxt, "Device ID: 0x%x\n", reg);
+	if (!(reg & 0x80)) {
+		cprintf(cxt, "Invalid device ID. Is the FUSB302 alive?\n");
+		cxt->hw = NULL;
+		return;
+	}
+
+	cprintf(cxt, "Init\n");
+	fusb302_tcpm_init(PORT(cxt));
+
+	fusb302_pd_reset(PORT(cxt));
+	fusb302_tcpm_set_rx_enable(PORT(cxt), 0);
+	fusb302_tcpm_set_cc(PORT(cxt), TYPEC_CC_OPEN);
+	sleep_ms(500);
+
+	tcpc_read(PORT(cxt), TCPC_REG_STATUS0, &reg);
+	cprintf(cxt, "STATUS0: 0x%x\n", reg);
+
+	gpio_set_irq_enabled_with_callback(PIN(cxt, FUSB_INT), GPIO_IRQ_LEVEL_LOW, true,
+					   fusb_int_handler);
+
+	evt_disconnect(cxt);
+	debug_poke(cxt);
+}
+
+static bool m1_pd_bmc_run_one(struct vdm_context *cxt)
+{
+	if (cxt->pending) {
+		handle_irq(cxt);
+		state_machine(cxt);
+		cxt->pending = false;
+		gpio_set_irq_enabled(PIN(cxt, FUSB_INT), GPIO_IRQ_LEVEL_LOW, true);
+	}
+		
+	return serial_handler(cxt) || cxt->pending;
+}
+
+void m1_pd_bmc_run(void)
+{
+	int i;
+
+	while (1) {
+		bool busy = false;
+
+		for (i = 0; i < CONFIG_USB_PD_PORT_COUNT; i++) {
+			if (vdm_contexts[i].hw)
+				busy |= m1_pd_bmc_run_one(&vdm_contexts[i]);
+		}
+
+		if (!busy)
+			__wfi();
+	}
+}