samus: pd: dual USB-PD port support for samus

Adds dual USB-PD port support for samus. Both ports are in dual-role
and can perform either role.

Both ports work fine when only one of the ports is in use. But,
still having problems with PD errors on the lower priority port (port
0). If you have a charger plugged into port 0, and a type-C USB dongle
plugged into port 1, then port 1 has higher priority, and in the
SRC_DISCONNECTED state, every 1.5 seconds when it sends source cap
packet, we occasionally drop pings on port 0, which results in a
lot of start/stop charging.

BUG=chrome-os-partner:28585
BRANCH=none
TEST=Tested on samus to make sure both ports work when I
plug in a charger and a type-C USB dongle with a pull-down on the CC
line. Tested on plankton and zinger to make sure PD works as expected.

Change-Id: Ie7bde3e258f5cd23a0b82b626c0993a45b0df074
Signed-off-by: Alec Berg <alecaberg@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/200750
Reviewed-by: Vic Yang <victoryang@chromium.org>
Tested-by: Vic Yang <victoryang@chromium.org>

1 files changed, 167 insertions, 157 deletions

diff --git a/chip/stm32/usb_pd_phy.c b/chip/stm32/usb_pd_phy.c
index 7d514840a1..bbe923b024 100644
--- a/chip/stm32/usb_pd_phy.c
+++ b/chip/stm32/usb_pd_phy.c
@@ -7,6 +7,7 @@
 #include "clock.h"
 #include "common.h"
 #include "console.h"
+#include "crc.h"
 #include "dma.h"
 #include "gpio.h"
 #include "hwtimer.h"
@@ -51,39 +52,42 @@
 #define NB_PERIOD(from, to) ((((to) - (from) + (PERIOD/2)) & 0xFF) / PERIOD)
 #define PERIOD_THRESHOLD ((PERIOD + 2*PERIOD) / 2)
 
-/* Timers used for TX and RX clocking */
-#define TIM_TX TIM_CLOCK_PD_TX
-#define TIM_RX TIM_CLOCK_PD_RX
+static struct pd_physical {
+	/* samples for the PD messages */
+	uint32_t raw_samples[DIV_ROUND_UP(PD_MAX_RAW_SIZE, sizeof(uint32_t))];
 
-#include "crc.h"
+	/* state of the bit decoder */
+	int d_toggle;
+	int d_lastlen;
+	uint32_t d_last;
+	int b_toggle;
 
-/* samples for the PD messages */
-static uint32_t raw_samples[DIV_ROUND_UP(PD_MAX_RAW_SIZE, sizeof(uint32_t))];
+	/* DMA structures for each PD port */
+	struct dma_option dma_tx_option;
+	struct dma_option dma_tim_option;
 
-/* state of the bit decoder */
-static int d_toggle;
-static int d_lastlen;
-static uint32_t d_last;
+	/* Pointers to timer register for each port */
+	timer_ctlr_t *tim_tx;
+	timer_ctlr_t *tim_rx;
+} pd_phy[PD_PORT_COUNT];
 
-void *pd_init_dequeue(void)
+void pd_init_dequeue(int port)
 {
 	/* preamble ends with 1 */
-	d_toggle = 0;
-	d_last = 0;
-	d_lastlen = 0;
-
-	return raw_samples;
+	pd_phy[port].d_toggle = 0;
+	pd_phy[port].d_last = 0;
+	pd_phy[port].d_lastlen = 0;
 }
 
-static int wait_bits(int nb)
+static int wait_bits(int port, int nb)
 {
 	int avail;
-	stm32_dma_chan_t *rx = dma_get_channel(DMAC_TIM_RX);
+	stm32_dma_chan_t *rx = dma_get_channel(DMAC_TIM_RX(port));
 
 	avail = dma_bytes_done(rx, PD_MAX_RAW_SIZE);
 	if (avail < nb) { /* no received yet ... */
 		while ((dma_bytes_done(rx, PD_MAX_RAW_SIZE) < nb)
-			&& !(STM32_TIM_SR(TIM_RX) & 4))
+			&& !(pd_phy[port].tim_rx->sr & 4))
 			; /* optimized for latency, not CPU usage ... */
 		if (dma_bytes_done(rx, PD_MAX_RAW_SIZE) < nb) {
 			CPRINTS("PD TMOUT RX %d/%d",
@@ -94,14 +98,14 @@ static int wait_bits(int nb)
 	return nb;
 }
 
-int pd_dequeue_bits(void *ctxt, int off, int len, uint32_t *val)
+int pd_dequeue_bits(int port, int off, int len, uint32_t *val)
 {
 	int w;
 	uint8_t cnt = 0xff;
-	uint8_t *samples = ctxt;
+	uint8_t *samples = (uint8_t *)pd_phy[port].raw_samples;
 
-	while ((d_lastlen < len) && (off < PD_MAX_RAW_SIZE - 1)) {
-		w = wait_bits(off + 2);
+	while ((pd_phy[port].d_lastlen < len) && (off < PD_MAX_RAW_SIZE - 1)) {
+		w = wait_bits(port, off + 2);
 		if (w < 0)
 			goto stream_err;
 		cnt = samples[off] - samples[off-1];
@@ -110,7 +114,7 @@ int pd_dequeue_bits(void *ctxt, int off, int len, uint32_t *val)
 		off++;
 		if (cnt <= PERIOD_THRESHOLD) {
 			/*
-			w = wait_bits(off + 1);
+			w = wait_bits(port, off + 1);
 			if (w < 0)
 				goto stream_err;
 			*/
@@ -121,13 +125,14 @@ int pd_dequeue_bits(void *ctxt, int off, int len, uint32_t *val)
 		}
 
 		/* enqueue the bit of the last period */
-		d_last = (d_last >> 1)
+		pd_phy[port].d_last = (pd_phy[port].d_last >> 1)
 		       | (cnt <= PERIOD_THRESHOLD ? 0x80000000 : 0);
-		d_lastlen++;
+		pd_phy[port].d_lastlen++;
 	}
 	if (off < PD_MAX_RAW_SIZE) {
-		*val = (d_last << (d_lastlen - len)) >> (32 - len);
-		d_lastlen -= len;
+		*val = (pd_phy[port].d_last << (pd_phy[port].d_lastlen - len))
+				>> (32 - len);
+		pd_phy[port].d_lastlen -= len;
 		return off;
 	} else {
 		return -1;
@@ -137,26 +142,26 @@ stream_err:
 	return -1;
 }
 
-int pd_find_preamble(void *ctxt)
+int pd_find_preamble(int port)
 {
 	int bit;
-	uint8_t *vals = ctxt;
+	uint8_t *vals = (uint8_t *)pd_phy[port].raw_samples;
 
 	/*
 	 * Detect preamble
 	 * Alternate 1-period 1-period & 2-period.
 	 */
 	uint32_t all = 0;
-	stm32_dma_chan_t *rx = dma_get_channel(DMAC_TIM_RX);
+	stm32_dma_chan_t *rx = dma_get_channel(DMAC_TIM_RX(port));
 
 	for (bit = 1; bit < PD_MAX_RAW_SIZE - 1; bit++) {
 		uint8_t cnt;
 		/* wait if the bit is not received yet ... */
 		if (PD_MAX_RAW_SIZE - rx->cndtr < bit + 1) {
 			while ((PD_MAX_RAW_SIZE - rx->cndtr < bit + 1) &&
-				!(STM32_TIM_SR(TIM_RX) & 4))
+				!(pd_phy[port].tim_rx->sr & 4))
 				;
-			if (STM32_TIM_SR(TIM_RX) & 4) {
+			if (pd_phy[port].tim_rx->sr & 4) {
 				CPRINTS("PD TMOUT RX %d/%d",
 					PD_MAX_RAW_SIZE - rx->cndtr, bit);
 				return -1;
@@ -172,28 +177,26 @@ int pd_find_preamble(void *ctxt)
 	return -1;
 }
 
-static int b_toggle;
-
-int pd_write_preamble(void *ctxt)
+int pd_write_preamble(int port)
 {
-	uint32_t *msg = ctxt;
+	uint32_t *msg = pd_phy[port].raw_samples;
 
 	/* 64-bit x2 preamble */
 	msg[0] = PD_PREAMBLE;
 	msg[1] = PD_PREAMBLE;
 	msg[2] = PD_PREAMBLE;
 	msg[3] = PD_PREAMBLE;
-	b_toggle = 0x3FF; /* preamble ends with 1 */
+	pd_phy[port].b_toggle = 0x3FF; /* preamble ends with 1 */
 	return 2*64;
 }
 
-int pd_write_sym(void *ctxt, int bit_off, uint32_t val10)
+int pd_write_sym(int port, int bit_off, uint32_t val10)
 {
-	uint32_t *msg = ctxt;
+	uint32_t *msg = pd_phy[port].raw_samples;
 	int word_idx = bit_off / 32;
 	int bit_idx = bit_off % 32;
-	uint32_t val = b_toggle ^ val10;
-	b_toggle = val & 0x200 ? 0x3FF : 0;
+	uint32_t val = pd_phy[port].b_toggle ^ val10;
+	pd_phy[port].b_toggle = val & 0x200 ? 0x3FF : 0;
 	if (bit_idx <= 22) {
 		if (bit_idx == 0)
 			msg[word_idx] = 0;
@@ -206,16 +209,16 @@ int pd_write_sym(void *ctxt, int bit_off, uint32_t val10)
 	return bit_off + 5*2;
 }
 
-int pd_write_last_edge(void *ctxt, int bit_off)
+int pd_write_last_edge(int port, int bit_off)
 {
-	uint32_t *msg = ctxt;
+	uint32_t *msg = pd_phy[port].raw_samples;
 	int word_idx = bit_off / 32;
 	int bit_idx = bit_off % 32;
 
 	if (bit_idx == 0)
 		msg[word_idx] = 0;
 
-	if (!b_toggle /* last bit was 0 */) {
+	if (!pd_phy[port].b_toggle /* last bit was 0 */) {
 		/* transition to 1, another 1, then 0 */
 		if (bit_idx == 31) {
 			msg[word_idx++] |= 1 << bit_idx;
@@ -231,9 +234,9 @@ int pd_write_last_edge(void *ctxt, int bit_off)
 }
 
 #ifdef CONFIG_COMMON_RUNTIME
-void pd_dump_packet(void *ctxt, const char *msg)
+void pd_dump_packet(int port, const char *msg)
 {
-	uint8_t *vals = ctxt;
+	uint8_t *vals = (uint8_t *)pd_phy[port].raw_samples;
 	int bit;
 
 	CPRINTF("ERR %s:\n000:- ", msg);
@@ -258,14 +261,9 @@ void pd_dump_packet(void *ctxt, const char *msg)
 
 /* --- SPI TX operation --- */
 
-static struct dma_option dma_tx_option = {
-	DMAC_SPI_TX, (void *)&SPI_REGS->dr,
-	STM32_DMA_CCR_MSIZE_8_BIT | STM32_DMA_CCR_PSIZE_8_BIT
-};
-
-void pd_tx_spi_init(void)
+void pd_tx_spi_init(int port)
 {
-	stm32_spi_regs_t *spi = SPI_REGS;
+	stm32_spi_regs_t *spi = SPI_REGS(port);
 
 	/* Enable Tx DMA for our first transaction */
 	spi->cr2 = STM32_SPI_CR2_TXDMAEN | STM32_SPI_CR2_DATASIZE(8);
@@ -292,36 +290,38 @@ void pd_tx_spi_init(void)
 #endif
 }
 
-void pd_tx_set_circular_mode(void)
+void pd_tx_set_circular_mode(int port)
 {
-	dma_tx_option.flags |= STM32_DMA_CCR_CIRC;
+	pd_phy[port].dma_tx_option.flags |= STM32_DMA_CCR_CIRC;
 }
 
-void pd_start_tx(void *ctxt, int polarity, int bit_len)
+void pd_start_tx(int port, int polarity, int bit_len)
 {
-	stm32_dma_chan_t *tx = dma_get_channel(DMAC_SPI_TX);
+	stm32_dma_chan_t *tx = dma_get_channel(DMAC_SPI_TX(port));
 
 	/* Initialize spi peripheral to prepare for transmission. */
-	pd_tx_spi_init();
+	pd_tx_spi_init(port);
 
 	/*
 	 * Set timer to one tick before reset so that the first tick causes
 	 * a rising edge on the output.
 	 */
-	STM32_TIM_CNT(TIM_TX) = TX_CLOCK_DIV - 1;
+	pd_phy[port].tim_tx->cnt = TX_CLOCK_DIV - 1;
 
 	/* update DMA configuration */
-	dma_prepare_tx(&dma_tx_option, DIV_ROUND_UP(bit_len, 8), ctxt);
+	dma_prepare_tx(&(pd_phy[port].dma_tx_option),
+			DIV_ROUND_UP(bit_len, 8),
+			pd_phy[port].raw_samples);
 	/* Flush data in write buffer so that DMA can get the latest data */
 	asm volatile("dmb;");
 
 	/* disable RX detection interrupt */
-	pd_rx_disable_monitoring();
+	pd_rx_disable_monitoring(port);
 
 	/* Kick off the DMA to send the data */
-	dma_clear_isr(DMAC_SPI_TX);
+	dma_clear_isr(DMAC_SPI_TX(port));
 #ifdef CONFIG_COMMON_RUNTIME
-	dma_enable_tc_interrupt(DMAC_SPI_TX);
+	dma_enable_tc_interrupt(DMAC_SPI_TX(port));
 #endif
 	dma_go(tx);
 
@@ -332,22 +332,22 @@ void pd_start_tx(void *ctxt, int polarity, int bit_len)
 	 * Call this last before enabling timer in order to meet spec on
 	 * timing between enabling TX and clocking out bits.
 	 */
-	pd_tx_enable(polarity);
+	pd_tx_enable(port, polarity);
 
 #ifndef CONFIG_USB_PD_TX_USES_SPI_MASTER
 	/* Start counting at 300Khz*/
-	STM32_TIM_CR1(TIM_TX) |= 1;
+	pd_phy[port].tim_tx->cr1 |= 1;
 #endif
 }
 
-void pd_tx_done(int polarity)
+void pd_tx_done(int port, int polarity)
 {
-	stm32_spi_regs_t *spi = SPI_REGS;
+	stm32_spi_regs_t *spi = SPI_REGS(port);
 
 	/* wait for DMA */
 #ifdef CONFIG_COMMON_RUNTIME
 	task_wait_event(DMA_TRANSFER_TIMEOUT_US);
-	dma_disable_tc_interrupt(DMAC_SPI_TX);
+	dma_disable_tc_interrupt(DMAC_SPI_TX(port));
 #endif
 
 	/* wait for real end of transmission */
@@ -382,157 +382,173 @@ void pd_tx_done(int polarity)
 #endif
 
 	/* put TX pins and reference in Hi-Z */
-	pd_tx_disable(polarity);
+	pd_tx_disable(port, polarity);
 
 #ifndef CONFIG_USB_PD_TX_USES_SPI_MASTER
 	/* Stop counting */
-	STM32_TIM_CR1(TIM_TX) &= ~1;
+	pd_phy[port].tim_tx->cr1 &= ~1;
 
 	/* Reset SPI to clear remaining data in buffer */
-	pd_tx_spi_reset();
+	pd_tx_spi_reset(port);
 #endif
 }
 
 /* --- RX operation using comparator linked to timer --- */
 
-static const struct dma_option dma_tim_option = {
-	DMAC_TIM_RX, (void *)&STM32_TIM_CCRx(TIM_RX, TIM_CCR_IDX),
-	STM32_DMA_CCR_MSIZE_8_BIT | STM32_DMA_CCR_PSIZE_16_BIT,
-};
-
-void pd_rx_start(void)
+void pd_rx_start(int port)
 {
 	/* start sampling the edges on the CC line using the RX timer */
-	dma_start_rx(&dma_tim_option, PD_MAX_RAW_SIZE, raw_samples);
+	dma_start_rx(&(pd_phy[port].dma_tim_option), PD_MAX_RAW_SIZE,
+			pd_phy[port].raw_samples);
 	/* enable TIM2 DMA requests */
-	STM32_TIM_EGR(TIM_RX) = 0x0001; /* reset counter / reload PSC */;
-	STM32_TIM_SR(TIM_RX) = 0; /* clear overflows */
-	STM32_TIM_CR1(TIM_RX) |= 1;
+	pd_phy[port].tim_rx->egr = 0x0001; /* reset counter / reload PSC */;
+	pd_phy[port].tim_rx->sr = 0; /* clear overflows */
+	pd_phy[port].tim_rx->cr1 |= 1;
 }
 
-void pd_rx_complete(void)
+void pd_rx_complete(int port)
 {
 	/* stop stampling TIM2 */
-	STM32_TIM_CR1(TIM_RX) &= ~1;
+	pd_phy[port].tim_rx->cr1 &= ~1;
 	/* stop DMA */
-	dma_disable(DMAC_TIM_RX);
+	dma_disable(DMAC_TIM_RX(port));
 }
 
-int pd_rx_started(void)
+int pd_rx_started(int port)
 {
 	/* is the sampling timer running ? */
-	return STM32_TIM_CR1(TIM_RX) & 1;
+	return pd_phy[port].tim_rx->cr1 & 1;
 }
 
-void pd_rx_enable_monitoring(void)
+void pd_rx_enable_monitoring(int port)
 {
 	/* clear comparator external interrupt */
-	STM32_EXTI_PR = EXTI_COMP_MASK;
-	/* clean up older comparator event */
-	task_clear_pending_irq(IRQ_COMP);
-	/* re-enable comparator interrupt to detect packets */
-	task_enable_irq(IRQ_COMP);
+	STM32_EXTI_PR = EXTI_COMP_MASK(port);
+	/* enable comparator external interrupt */
+	STM32_EXTI_IMR |= EXTI_COMP_MASK(port);
 }
 
-void pd_rx_disable_monitoring(void)
+void pd_rx_disable_monitoring(int port)
 {
-	/* stop monitoring RX during sampling */
-	task_disable_irq(IRQ_COMP);
+	/* disable comparator external interrupt */
+	STM32_EXTI_IMR &= ~EXTI_COMP_MASK(port);
 	/* clear comparator external interrupt */
-	STM32_EXTI_PR = EXTI_COMP_MASK;
+	STM32_EXTI_PR = EXTI_COMP_MASK(port);
 }
 
 /* detect an edge on the PD RX pin */
 void pd_rx_handler(void)
 {
-	/* start sampling */
-	pd_rx_start();
-	/* ignore the comparator IRQ until we are done with current message */
-	pd_rx_disable_monitoring();
-	/* trigger the analysis in the task */
-	pd_rx_event();
+	int pending, i;
+	pending = STM32_EXTI_PR;
+
+	for (i = 0; i < PD_PORT_COUNT; i++) {
+		if (pending & EXTI_COMP_MASK(i)) {
+			/* start sampling */
+			pd_rx_start(i);
+			/*
+			 * ignore the comparator IRQ until we are done with
+			 * current message
+			 */
+			pd_rx_disable_monitoring(i);
+			/* trigger the analysis in the task */
+			pd_rx_event(i);
+		}
+	}
 }
 #ifndef BOARD_ZINGER
 DECLARE_IRQ(STM32_IRQ_COMP, pd_rx_handler, 1);
 #endif
 
 /* --- release hardware --- */
-void pd_hw_release(void)
+void pd_hw_release(int port)
 {
-	__hw_timer_enable_clock(TIM_RX, 0);
-	__hw_timer_enable_clock(TIM_TX, 0);
-	dma_disable(DMAC_SPI_TX);
+	__hw_timer_enable_clock(TIM_CLOCK_PD_RX(port), 0);
+	__hw_timer_enable_clock(TIM_CLOCK_PD_TX(port), 0);
+	dma_disable(DMAC_SPI_TX(port));
 }
 
 /* --- Startup initialization --- */
-void *pd_hw_init(void)
+void pd_hw_init(int port)
 {
+	struct pd_physical *phy = &pd_phy[port];
 	/* set 40 MHz pin speed on communication pins */
-	pd_set_pins_speed();
+	pd_set_pins_speed(port);
 
 	/* --- SPI init --- */
 
 	/* Enable clocks to SPI module */
-	spi_enable_clock();
-
-	/* Initialize TX pins and put them in Hi-Z */
-	pd_tx_init();
+	spi_enable_clock(port);
 
 	/* Initialize SPI peripheral registers */
-	pd_tx_spi_init();
+	pd_tx_spi_init(port);
 
 	/* configure TX DMA */
-	dma_prepare_tx(&dma_tx_option, PD_MAX_RAW_SIZE, raw_samples);
+	phy->dma_tx_option.channel = DMAC_SPI_TX(port);
+	phy->dma_tx_option.periph = (void *)&SPI_REGS(port)->dr;
+	phy->dma_tx_option.flags = STM32_DMA_CCR_MSIZE_8_BIT |
+				    STM32_DMA_CCR_PSIZE_8_BIT;
+	dma_prepare_tx(&(phy->dma_tx_option), PD_MAX_RAW_SIZE,
+			phy->raw_samples);
+
+	/* configure RX DMA */
+	phy->dma_tim_option.channel = DMAC_TIM_RX(port);
+	phy->dma_tim_option.periph = (void *)(TIM_RX_CCR_REG(port));
+	phy->dma_tim_option.flags = STM32_DMA_CCR_MSIZE_8_BIT |
+				     STM32_DMA_CCR_PSIZE_16_BIT;
+
+	/* configure registers used for timers */
+	phy->tim_tx = (void *)TIM_REG_TX(port);
+	phy->tim_rx = (void *)TIM_REG_RX(port);
 
 #ifndef CONFIG_USB_PD_TX_USES_SPI_MASTER
 	/* --- set the TX timer with updates at 600KHz (BMC frequency) --- */
-	__hw_timer_enable_clock(TIM_TX, 1);
+	__hw_timer_enable_clock(TIM_CLOCK_PD_TX(port), 1);
 	/* Timer configuration */
-	STM32_TIM_CR1(TIM_TX) = 0x0000;
-	STM32_TIM_CR2(TIM_TX) = 0x0000;
-	STM32_TIM_DIER(TIM_TX) = 0x0000;
+	phy->tim_tx->cr1 = 0x0000;
+	phy->tim_tx->cr2 = 0x0000;
+	phy->tim_tx->dier = 0x0000;
 	/* Auto-reload value : 600000 Khz overflow */
-	STM32_TIM_ARR(TIM_TX) = TX_CLOCK_DIV;
+	phy->tim_tx->arr = TX_CLOCK_DIV;
 	/* 50% duty cycle on the output */
-	STM32_TIM_CCR1(TIM_TX) = STM32_TIM_ARR(TIM_TX) / 2;
+	phy->tim_tx->ccr[1] = phy->tim_tx->arr / 2;
 	/* Timer CH1 output configuration */
-	STM32_TIM_CCMR1(TIM_TX) = (6 << 4) | (1 << 3);
-	STM32_TIM_CCER(TIM_TX) = 1;
-	STM32_TIM_BDTR(TIM_TX) = 0x8000;
+	phy->tim_tx->ccmr1 = (6 << 4) | (1 << 3);
+	phy->tim_tx->ccer = 1;
+	phy->tim_tx->bdtr = 0x8000;
 	/* set prescaler to /1 */
-	STM32_TIM_PSC(TIM_TX) = 0;
+	phy->tim_tx->psc = 0;
 	/* Reload the pre-scaler and reset the counter */
-	STM32_TIM_EGR(TIM_TX) = 0x0001;
+	phy->tim_tx->egr = 0x0001;
 #endif
 
 	/* --- set counter for RX timing : 2.4Mhz rate, free-running --- */
-	__hw_timer_enable_clock(TIM_RX, 1);
+	__hw_timer_enable_clock(TIM_CLOCK_PD_RX(port), 1);
 	/* Timer configuration */
-	STM32_TIM_CR1(TIM_RX) = 0x0000;
-	STM32_TIM_CR2(TIM_RX) = 0x0000;
-	STM32_TIM_DIER(TIM_RX) = 0x0000;
+	phy->tim_rx->cr1 = 0x0000;
+	phy->tim_rx->cr2 = 0x0000;
+	phy->tim_rx->dier = 0x0000;
 	/* Auto-reload value : 16-bit free running counter */
-	STM32_TIM_ARR(TIM_RX) = 0xFFFF;
+	phy->tim_rx->arr = 0xFFFF;
 
 	/* Timeout for message receive : 2.7ms */
-	STM32_TIM_CCR2(TIM_RX) = 2400000 * 27 / 10000;
+	phy->tim_rx->ccr[2] = 2400000 * 27 / 10000;
 	/* Timer ICx input configuration */
-#if TIM_CCR_IDX == 1
-	STM32_TIM_CCMR1(TIM_RX) = TIM_CCR_CS << 0;
-#elif TIM_CCR_IDX == 4
-	STM32_TIM_CCMR2(TIM_RX) = TIM_CCR_CS << 8;
-#else
-#error Unsupported RX timer capture input
-#endif
-	STM32_TIM_CCER(TIM_RX) = 0xB << ((TIM_CCR_IDX - 1) * 4);
+	if (TIM_CCR_IDX(port) == 1)
+		phy->tim_rx->ccmr1 |= TIM_CCR_CS << 0;
+	else
+		/*  Unsupported RX timer capture input */
+		ASSERT(0);
+
+	phy->tim_rx->ccer = 0xB << ((TIM_CCR_IDX(port) - 1) * 4);
 	/* configure DMA request on CCRx update */
-	STM32_TIM_DIER(TIM_RX) |= 1 << (8 + TIM_CCR_IDX); /* CCxDE */;
+	phy->tim_rx->dier |= 1 << (8 + TIM_CCR_IDX(port)); /* CCxDE */;
 	/* set prescaler to /26 (F=1.2Mhz, T=0.8us) */
-	STM32_TIM_PSC(TIM_RX) = (clock_get_freq() / 2400000) - 1;
-	/* Reload the pre-scaler and reset the counter */
-	STM32_TIM_EGR(TIM_RX) = 0x0001 | (1 << TIM_CCR_IDX) /* clear CCRx */;
+	phy->tim_rx->psc = (clock_get_freq() / 2400000) - 1;
+	/* Reload the pre-scaler and reset the counter (clear CCRx) */
+	phy->tim_rx->egr = 0x0001 | (1 << TIM_CCR_IDX(port));
 	/* clear update event from reloading */
-	STM32_TIM_SR(TIM_RX) = 0;
+	phy->tim_rx->sr = 0;
 
 	/* --- DAC configuration for comparator at 850mV --- */
 #ifdef CONFIG_PD_USE_DAC_AS_REF
@@ -547,23 +563,18 @@ void *pd_hw_init(void)
 	/* --- COMP2 as comparator for RX vs Vmid = 850mV --- */
 #ifdef CONFIG_USB_PD_INTERNAL_COMP
 #if defined(CHIP_FAMILY_STM32F0)
-	/* 40 MHz pin speed on PA0 and PA4 */
-	STM32_GPIO_OSPEEDR(GPIO_A) |= 0x303;
 	/* turn on COMP/SYSCFG */
 	STM32_RCC_APB2ENR |= 1 << 0;
 	/* currently in hi-speed mode : TODO revisit later, INM = PA0(INM6) */
 	STM32_COMP_CSR = STM32_COMP_CMP1MODE_LSPEED |
 			 STM32_COMP_CMP1INSEL_INM6 |
-			 STM32_COMP_CMP1OUTSEL_TIM1_IC1 |
+			 CMP1OUTSEL |
 			 STM32_COMP_CMP1HYST_HI |
 			 STM32_COMP_CMP2MODE_LSPEED |
 			 STM32_COMP_CMP2INSEL_INM6 |
-			 STM32_COMP_CMP2OUTSEL_TIM1_IC1 |
+			 CMP2OUTSEL |
 			 STM32_COMP_CMP2HYST_HI;
 #elif defined(CHIP_FAMILY_STM32L)
-	/* 40 MHz pin speed on PB4 */
-	STM32_GPIO_OSPEEDR(GPIO_B) |= 0x300;
-
 	STM32_RCC_APB1ENR |= 1 << 31; /* turn on COMP */
 
 	STM32_COMP_CSR = STM32_COMP_OUTSEL_TIM2_IC4 | STM32_COMP_INSEL_DAC_OUT1
@@ -576,15 +587,14 @@ void *pd_hw_init(void)
 #endif /* CONFIG_USB_PD_INTERNAL_COMP */
 	/* DBG */usleep(250000);
 	/* comparator interrupt setup */
-	EXTI_XTSR |= EXTI_COMP_MASK;
-	STM32_EXTI_IMR |= EXTI_COMP_MASK;
+	EXTI_XTSR |= EXTI_COMP_MASK(port);
+	STM32_EXTI_IMR |= EXTI_COMP_MASK(port);
 	task_enable_irq(IRQ_COMP);
 
 	CPRINTS("USB PD initialized");
-	return raw_samples;
 }
 
-void pd_set_clock(int freq)
+void pd_set_clock(int port, int freq)
 {
-	STM32_TIM_ARR(TIM_TX) = clock_get_freq() / (2*freq);
+	pd_phy[port].tim_tx->arr = clock_get_freq() / (2*freq);
 }