Twinkie: add packet injection capability

Add commands to send PD packets and to tweak individual parameters (TX
clock frequency, RX detection threshold, resistors on CCx).

Signed-off-by: Vincent Palatin <vpalatin@chromium.org>

BRANCH=none
BUG=chrome-os-partner:28337
TEST=verify that the PD sniffing is still working by dumping traffic
between Zinger and Samus.
Connect Twinkie to Zinger, set Rd by using "tw res NONE RD" and see VBUS
going to 5V (reading it using "ina 0").
Send a BIST mode 2 request using the following command :
tw send 2 0x1043 50000000
and see the other end starting sending BIST.

Change-Id: I3c8ddf858435ac1c17a43f59351bbaa69603a209
Reviewed-on: https://chromium-review.googlesource.com/227778
Reviewed-by: Todd Broch <tbroch@chromium.org>
Commit-Queue: Vincent Palatin <vpalatin@chromium.org>
Tested-by: Vincent Palatin <vpalatin@chromium.org>

11 files changed, 683 insertions, 14 deletions

diff --git a/board/twinkie/board.h b/board/twinkie/board.h
index edfcc4800b..b748104ad2 100644
--- a/board/twinkie/board.h
+++ b/board/twinkie/board.h
@@ -18,14 +18,15 @@
 #define CONFIG_USB
 #define CONFIG_USB_CONSOLE
 
-#ifdef HAS_TASK_PD /* PD message injector mode */
 #define CONFIG_USB_POWER_DELIVERY
 #define CONFIG_USB_PD_DUAL_ROLE
 #define CONFIG_USB_PD_INTERNAL_COMP
 #define CONFIG_PD_USE_DAC_AS_REF
 #define CONFIG_HW_CRC
-#else /* PD sniffer mode */
+
+#ifndef HAS_TASK_PD /* PD sniffer mode */
 #undef CONFIG_DMA_DEFAULT_HANDLERS
+#define CONFIG_USB_PD_TX_PHY_ONLY
 #endif
 
 #define CONFIG_ADC
@@ -52,6 +53,10 @@
 
 #ifndef __ASSEMBLER__
 
+int wait_packet(int pol, uint32_t min_edges, uint32_t timeout_us);
+
+void recording_enable(uint8_t mask);
+
 /* Timer selection */
 #define TIM_CLOCK_MSB  3
 #define TIM_CLOCK_LSB 15
diff --git a/board/twinkie/build.mk b/board/twinkie/build.mk
index d920169312..f9b5e7d973 100644
--- a/board/twinkie/build.mk
+++ b/board/twinkie/build.mk
@@ -11,5 +11,5 @@ CHIP_FAMILY:=stm32f0
 CHIP_VARIANT:=stm32f07x
 
 board-y=board.o
-board-$(HAS_TASK_SNIFFER)+=sniffer.o
+board-$(HAS_TASK_SNIFFER)+=sniffer.o injector.o
 board-$(HAS_TASK_PD)+=usb_pd_policy.o
diff --git a/board/twinkie/ec.tasklist b/board/twinkie/ec.tasklist
index a766187877..1bc0b5212c 100644
--- a/board/twinkie/ec.tasklist
+++ b/board/twinkie/ec.tasklist
@@ -18,8 +18,8 @@
  */
 #define CONFIG_TASK_LIST \
 	TASK_ALWAYS(HOOKS, hook_task, NULL, TASK_STACK_SIZE) \
+	TASK_ALWAYS(CONSOLE, console_task, NULL, LARGER_TASK_STACK_SIZE) \
 	TASK_ALWAYS(SNIFFER, sniffer_task, NULL, TASK_STACK_SIZE) \
-	TASK_ALWAYS(CONSOLE, console_task, NULL, TASK_STACK_SIZE) \
 	/*Disabled: TASK_ALWAYS(PD, pd_task, NULL, LARGER_TASK_STACK_SIZE) */
 /*
  * To get Twinkie to behave as a USB Power Delivery consumer/provider
diff --git a/board/twinkie/injector.c b/board/twinkie/injector.c
new file mode 100644
index 0000000000..4f1ece6f78
--- /dev/null
+++ b/board/twinkie/injector.c
@@ -0,0 +1,515 @@
+/* Copyright (c) 2014 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "adc.h"
+#include "common.h"
+#include "console.h"
+#include "dma.h"
+#include "gpio.h"
+#include "hooks.h"
+#include "hwtimer.h"
+#include "injector.h"
+#include "registers.h"
+#include "task.h"
+#include "timer.h"
+#include "usb_pd.h"
+#include "usb_pd_config.h"
+#include "util.h"
+#include "watchdog.h"
+
+/* FSM command/data buffer */
+static uint32_t inj_cmds[INJ_CMD_COUNT];
+
+/* Current polarity for sending operations */
+static enum inj_pol inj_polarity = INJ_POL_CC1;
+
+/*
+ * CCx Resistors control definition
+ *
+ * Resistor control GPIOs :
+ * CC1_RA       A8
+ * CC1_RPUSB    A13
+ * CC1_RP1A5    A14
+ * CC1_RP3A0    A15
+ * CC2_RPUSB    B0
+ * CC1_RD       B5
+ * CC2_RD       B8
+ * CC2_RA       B15
+ * CC2_RP1A5    C14
+ * CC2_RP3A0    C15
+ */
+static const struct res_cfg {
+	const char *name;
+	struct config {
+		uint32_t port;
+		uint32_t mask;
+		uint32_t flags;
+	} cfgs[2];
+} res_cfg[] = {
+	[INJ_RES_NONE]  = {"NONE"},
+	[INJ_RES_RA]    = {"RA", {{GPIO_A, 0x0100, GPIO_ODR_LOW},
+				  {GPIO_B, 0x8000, GPIO_ODR_LOW} } },
+	[INJ_RES_RD]    = {"RD", {{GPIO_B, 0x0020, GPIO_ODR_LOW},
+				  {GPIO_B, 0x0100, GPIO_ODR_LOW} } },
+	[INJ_RES_RPUSB] = {"RPUSB", {{GPIO_A, 0x2000, GPIO_OUT_HIGH},
+				     {GPIO_B, 0x0001, GPIO_OUT_HIGH} } },
+	[INJ_RES_RP1A5] = {"RP1A5", {{GPIO_A, 0x4000, GPIO_OUT_HIGH},
+				     {GPIO_C, 0x4000, GPIO_OUT_HIGH} } },
+	[INJ_RES_RP3A0] = {"RP3A0", {{GPIO_A, 0x8000, GPIO_OUT_HIGH},
+				     {GPIO_C, 0x8000, GPIO_OUT_HIGH} } },
+};
+
+#define PD_SRC_RD_THRESHOLD  200 /* mV */
+#define CC_RA(cc)  (cc < PD_SRC_RD_THRESHOLD)
+#define CC_RD(cc) ((cc > PD_SRC_RD_THRESHOLD) && (cc < PD_SRC_VNC))
+#define GET_POLARITY(cc1, cc2) (CC_RD(cc2) || CC_RA(cc1))
+
+/* Stub the function as we are not using the RX path */
+void pd_set_max_voltage(unsigned mv)
+{
+}
+
+/* we don't have the default DMA handlers */
+void dma_event_interrupt_channel_3(void)
+{
+	if (STM32_DMA1_REGS->isr & STM32_DMA_ISR_TCIF(STM32_DMAC_CH3)) {
+		dma_clear_isr(STM32_DMAC_CH3);
+		task_wake(TASK_ID_CONSOLE);
+	}
+}
+DECLARE_IRQ(STM32_IRQ_DMA_CHANNEL_2_3, dma_event_interrupt_channel_3, 3);
+
+static void twinkie_init(void)
+{
+	/* configure TX clock pins */
+	gpio_config_module(MODULE_USB_PD, 1);
+	/* Initialize physical layer */
+	pd_hw_init(0);
+}
+DECLARE_HOOK(HOOK_INIT, twinkie_init, HOOK_PRIO_DEFAULT);
+
+/* ------ Helper functions ------ */
+
+static int send_message(int polarity, uint16_t header,
+			uint8_t cnt, const uint32_t *data)
+{
+	int bit_len;
+
+	bit_len = prepare_message(0, header, cnt, data);
+	/* Transmit the packet */
+	pd_start_tx(0, polarity, bit_len);
+	pd_tx_done(0, polarity);
+
+	return bit_len;
+}
+
+static int send_hrst(int polarity)
+{
+	int off;
+	/* 64-bit preamble */
+	off = pd_write_preamble(0);
+	/* Hard-Reset: 3x RST-1 + 1x RST-2 */
+	off = pd_write_sym(0, off, 0b0011010101); /* RST-1 = 00111 */
+	off = pd_write_sym(0, off, 0b0011010101); /* RST-1 = 00111 */
+	off = pd_write_sym(0, off, 0b0011010101); /* RST-1 = 00111 */
+	off = pd_write_sym(0, off, 0b0101001101); /* RST-2 = 11001 */
+	/* Ensure that we have a final edge */
+	off = pd_write_last_edge(0, off);
+	/* Transmit the packet */
+	pd_start_tx(0, polarity, off);
+	pd_tx_done(0, polarity);
+
+	return off;
+}
+
+static void set_resistor(int pol, enum inj_res res)
+{
+	/* reset everything on one CC to high impedance */
+	gpio_set_flags_by_mask(GPIO_A, pol ? 0x0000 : 0xE100, GPIO_ODR_HIGH);
+	gpio_set_flags_by_mask(GPIO_B, pol ? 0x8101 : 0x0020, GPIO_ODR_HIGH);
+	gpio_set_flags_by_mask(GPIO_C, pol ? 0xC000 : 0x0000, GPIO_ODR_HIGH);
+	/* connect the resistor if needed */
+	if (res_cfg[res].cfgs[pol].mask)
+		gpio_set_flags_by_mask(res_cfg[res].cfgs[pol].port,
+					res_cfg[res].cfgs[pol].mask,
+					res_cfg[res].cfgs[pol].flags);
+}
+
+static enum inj_pol guess_polarity(enum inj_pol pol)
+{
+	int cc1_volt, cc2_volt;
+	/* polarity forced by the user */
+	if (pol == INJ_POL_CC1 || pol == INJ_POL_CC2)
+		return pol;
+	/* Auto-detection */
+	cc1_volt = pd_adc_read(0, 0);
+	cc2_volt = pd_adc_read(0, 1);
+	return GET_POLARITY(cc1_volt, cc2_volt);
+}
+
+/* ------ FSM commands ------ */
+
+static void fsm_send(uint32_t w)
+{
+	uint16_t header = INJ_ARG0(w);
+	int idx = INJ_ARG1(w);
+	uint8_t cnt = INJ_ARG2(w);
+
+	/* Buffer overflow */
+	if (idx > INJ_CMD_COUNT)
+		return;
+
+	send_message(inj_polarity, header, cnt, inj_cmds + idx);
+}
+
+static void fsm_wave(uint32_t w)
+{
+	uint16_t bit_len = INJ_ARG0(w);
+	int idx = INJ_ARG1(w);
+	int off = 0;
+	int nbwords = DIV_ROUND_UP(bit_len, 32);
+	int i;
+
+	/* Buffer overflow */
+	if (idx + nbwords > INJ_CMD_COUNT)
+		return;
+
+	for (i = idx; i < idx + nbwords; i++)
+		off = encode_word(0, off, inj_cmds[i]);
+	/* Ensure that we have a final edge */
+	off = pd_write_last_edge(0, bit_len);
+	/* Transmit the packet */
+	pd_start_tx(0, inj_polarity, off);
+	pd_tx_done(0, inj_polarity);
+}
+
+static void fsm_wait(uint32_t w)
+{
+	uint32_t timeout_ms = INJ_ARG0(w);
+	uint32_t min_edges = INJ_ARG12(w);
+
+	wait_packet(inj_polarity,  min_edges, timeout_ms * 1000);
+}
+
+static void fsm_get(uint32_t w)
+{
+	int store_idx = INJ_ARG0(w);
+	int param_idx = INJ_ARG1(w);
+	uint32_t *store_ptr = inj_cmds + store_idx;
+
+	/* Buffer overflow */
+	if (store_idx > INJ_CMD_COUNT)
+		return;
+
+	switch (param_idx) {
+	case INJ_GET_CC:
+		*store_ptr = pd_adc_read(0, 0) | (pd_adc_read(0, 1) << 16);
+		break;
+	case INJ_GET_VBUS:
+		*store_ptr = (ina2xx_get_voltage(0) & 0xffff) |
+			 ((ina2xx_get_current(0) & 0xffff) << 16);
+		break;
+	case INJ_GET_VCONN:
+		*store_ptr = (ina2xx_get_voltage(1) & 0xffff) |
+			 ((ina2xx_get_current(1) & 0xffff) << 16);
+		break;
+	case INJ_GET_POLARITY:
+		*store_ptr = inj_polarity;
+		break;
+	default:
+		/* Do nothing */
+		break;
+	}
+}
+
+static void fsm_set(uint32_t w)
+{
+	int val = INJ_ARG0(w);
+	int idx = INJ_ARG1(w);
+
+	switch (idx) {
+	case INJ_SET_RESISTOR1:
+	case INJ_SET_RESISTOR2:
+		set_resistor(idx - INJ_SET_RESISTOR1, val);
+		break;
+	case INJ_SET_RECORD:
+		recording_enable(val);
+		break;
+	case INJ_SET_TX_SPEED:
+		pd_set_clock(0, val * 1000);
+		break;
+	case INJ_SET_RX_THRESH:
+		/* set DAC voltage (Vref = 3.3V) */
+		STM32_DAC_DHR12RD = val * 4096 / 3300;
+		break;
+	case INJ_SET_POLARITY:
+		inj_polarity = guess_polarity(val);
+		break;
+	default:
+		/* Do nothing */
+		break;
+	}
+}
+
+static int fsm_run(int index)
+{
+	while (index < INJ_CMD_COUNT) {
+		uint32_t w = inj_cmds[index];
+		int cmd = INJ_CMD(w);
+		switch (cmd) {
+		case INJ_CMD_END:
+			return index;
+		case INJ_CMD_SEND:
+			fsm_send(w);
+			break;
+		case INJ_CMD_WAVE:
+			fsm_wave(w);
+			break;
+		case INJ_CMD_HRST:
+			send_hrst(inj_polarity);
+			break;
+		case INJ_CMD_WAIT:
+			fsm_wait(w);
+			break;
+		case INJ_CMD_GET:
+			fsm_get(w);
+			break;
+		case INJ_CMD_SET:
+			fsm_set(w);
+			break;
+		case INJ_CMD_JUMP:
+			index = INJ_ARG0(w);
+			continue; /* do not increment index */
+		case INJ_CMD_NOP:
+		default:
+			/* Do nothing */
+			break;
+		}
+		index += 1;
+		watchdog_reload();
+	}
+	return index;
+}
+
+/* ------ Console commands ------ */
+
+static int hex8tou32(char *str, uint32_t *val)
+{
+	char *ptr = str;
+	uint32_t tmp = 0;
+
+	while (*ptr) {
+		char c = *ptr++;
+		if (c >= '0' && c <= '9')
+			tmp = (tmp << 4) + (c - '0');
+		else if (c >= 'A' && c <= 'F')
+			tmp = (tmp << 4) + (c - 'A' + 10);
+		else if (c >= 'a' && c <= 'f')
+			tmp = (tmp << 4) + (c - 'a' + 10);
+		else
+			return EC_ERROR_INVAL;
+	}
+	if (ptr != str + 8)
+		return EC_ERROR_INVAL;
+	*val = tmp;
+	return EC_SUCCESS;
+}
+
+static int cmd_fsm(int argc, char **argv)
+{
+	int index;
+	char *e;
+
+	if (argc < 1)
+		return EC_ERROR_PARAM2;
+
+	index = strtoi(argv[0], &e, 10);
+	if (*e)
+		return EC_ERROR_PARAM2;
+	index = fsm_run(index);
+	ccprintf("FSM Done %d\n", index);
+
+	return EC_SUCCESS;
+}
+
+
+static int cmd_send(int argc, char **argv)
+{
+	int pol, cnt, i;
+	uint16_t header;
+	uint32_t data[VDO_MAX_SIZE-1];
+	char *e;
+	int bit_len;
+
+	cnt = argc - 2;
+	if (argc < 2 || cnt > VDO_MAX_SIZE)
+		return EC_ERROR_PARAM_COUNT;
+
+	pol = strtoi(argv[0], &e, 10) - 1;
+	if (*e || pol > 1 || pol < 0)
+		return EC_ERROR_PARAM2;
+	header = strtoi(argv[1], &e, 16);
+	if (*e)
+		return EC_ERROR_PARAM3;
+
+	for (i = 0; i < cnt; i++)
+		if (hex8tou32(argv[i+2], data + i))
+			return EC_ERROR_INVAL;
+
+	bit_len = send_message(pol, header, cnt, data);
+	ccprintf("Sent CC%d %04x + %d = %d\n", pol + 1, header, cnt, bit_len);
+
+	return EC_SUCCESS;
+}
+
+static int cmd_cc_level(int argc, char **argv)
+{
+	ccprintf("CC1 = %d mV ; CC2 = %d mV\n",
+		pd_adc_read(0, 0), pd_adc_read(0, 1));
+
+	return EC_SUCCESS;
+}
+
+static int cmd_resistor(int argc, char **argv)
+{
+	int p, r;
+
+	if (argc < 2)
+		return EC_ERROR_PARAM_COUNT;
+
+	for (p = 0; p < 2; p++) {
+		int is_set = 0;
+		for (r = 0; r < ARRAY_SIZE(res_cfg); r++)
+			if (strcasecmp(res_cfg[r].name, argv[p]) == 0) {
+				set_resistor(p, r);
+				is_set = 1;
+				break;
+			}
+		/* Unknown name : set to No resistor */
+		if (!is_set)
+			set_resistor(p, INJ_RES_NONE);
+	}
+	return EC_SUCCESS;
+}
+
+static int cmd_tx_clock(int argc, char **argv)
+{
+	int freq;
+	char *e;
+
+	if (argc < 1)
+		return EC_ERROR_PARAM2;
+
+	freq = strtoi(argv[0], &e, 10);
+	if (*e)
+		return EC_ERROR_PARAM2;
+	pd_set_clock(0, freq);
+	ccprintf("TX frequency = %d Hz\n", freq);
+
+	return EC_SUCCESS;
+}
+
+static int cmd_rx_threshold(int argc, char **argv)
+{
+	int mv;
+	char *e;
+
+	if (argc < 1)
+		return EC_ERROR_PARAM2;
+
+	mv = strtoi(argv[0], &e, 10);
+	if (*e)
+		return EC_ERROR_PARAM2;
+
+	/* set DAC voltage (Vref = 3.3V) */
+	STM32_DAC_DHR12RD = mv * 4096 / 3300;
+	ccprintf("RX threshold = %d mV\n", mv);
+
+	return EC_SUCCESS;
+}
+
+static int cmd_ina_dump(int argc, char **argv, int index)
+{
+	ccprintf("%s = %d mV ; %d mA\n", index == 0 ? "VBUS" : "VCONN",
+		ina2xx_get_voltage(index), ina2xx_get_current(index));
+
+	return EC_SUCCESS;
+}
+
+static int cmd_bufwr(int argc, char **argv)
+{
+	int idx, cnt, i;
+	char *e;
+
+	cnt = argc - 1;
+	if (argc < 2 || cnt > INJ_CMD_COUNT)
+		return EC_ERROR_PARAM_COUNT;
+
+	idx = strtoi(argv[0], &e, 10);
+	if (*e || idx + cnt > INJ_CMD_COUNT)
+		return EC_ERROR_PARAM2;
+
+	for (i = 0; i < cnt; i++)
+		if (hex8tou32(argv[i+1], inj_cmds + idx + i))
+			return EC_ERROR_INVAL;
+
+	return EC_SUCCESS;
+}
+
+static int cmd_bufrd(int argc, char **argv)
+{
+	int idx, i;
+	int cnt = 1;
+	char *e;
+
+	if (argc < 1)
+		return EC_ERROR_PARAM_COUNT;
+
+	idx = strtoi(argv[0], &e, 10);
+	if (*e || idx > INJ_CMD_COUNT)
+		return EC_ERROR_PARAM2;
+
+	if (argc >= 2)
+		cnt = strtoi(argv[0], &e, 10);
+		if (*e || idx + cnt > INJ_CMD_COUNT)
+			return EC_ERROR_PARAM3;
+
+	for (i = idx; i < idx + cnt; i++)
+		ccprintf("%08x ", inj_cmds[i]);
+	ccprintf("\n");
+
+	return EC_SUCCESS;
+}
+
+static int command_tw(int argc, char **argv)
+{
+	if (!strcasecmp(argv[1], "send"))
+		return cmd_send(argc - 2, argv + 2);
+	else if (!strcasecmp(argv[1], "fsm"))
+		return cmd_fsm(argc - 2, argv + 2);
+	else if (!strcasecmp(argv[1], "bufwr"))
+		return cmd_bufwr(argc - 2, argv + 2);
+	else if (!strcasecmp(argv[1], "bufrd"))
+		return cmd_bufrd(argc - 2, argv + 2);
+	else if (!strcasecmp(argv[1], "cc"))
+		return cmd_cc_level(argc - 2, argv + 2);
+	else if (!strncasecmp(argv[1], "resistor", 3))
+		return cmd_resistor(argc - 2, argv + 2);
+	else if (!strcasecmp(argv[1], "txclock"))
+		return cmd_tx_clock(argc - 2, argv + 2);
+	else if (!strncasecmp(argv[1], "rxthresh", 8))
+		return cmd_rx_threshold(argc - 2, argv + 2);
+	else if (!strcasecmp(argv[1], "vbus"))
+		return cmd_ina_dump(argc - 2, argv + 2, 0);
+	else if (!strcasecmp(argv[1], "vconn"))
+		return cmd_ina_dump(argc - 2, argv + 2, 1);
+	else
+		return EC_ERROR_PARAM1;
+
+	return EC_SUCCESS;
+}
+DECLARE_CONSOLE_COMMAND(twinkie, command_tw,
+			"[send|fsm|cc|resistor|txclock|rxthresh|vbus|vconn]",
+			"Manual Twinkie tweaking", NULL);
diff --git a/board/twinkie/injector.h b/board/twinkie/injector.h
new file mode 100644
index 0000000000..0e360a2fdc
--- /dev/null
+++ b/board/twinkie/injector.h
@@ -0,0 +1,79 @@
+/* Copyright (c) 2014 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.
+ */
+
+#ifndef __INJECTOR_H
+#define __INJECTOR_H
+
+/*
+ * Finite state machine definition for sending complex sequences
+ *
+ * the 32-bit commands have the following definition :
+ *    [31:28]   [27:0]
+ *    Command    arg
+ *   INJ_CMD_x
+ *                  [27:16]     [15:0]
+ *                   arg12       arg0
+ *              [27:24] [23:16] [15:0]
+ *               arg2     arg1   arg0
+ */
+
+/* Macros to extract values from FSM command words */
+#define INJ_CMD(w) ((w) >> 28)
+#define INJ_ARG(w) ((w) & 0x0FFFFFFF)
+#define INJ_ARG0(w) ((w) & 0x0000FFFF)
+#define INJ_ARG1(w) (((w) >> 16) & 0xFF)
+#define INJ_ARG2(w) (((w) >> 24) & 0xF)
+#define INJ_ARG12(w) (((w) >> 16) & 0xFFF)
+
+enum inj_cmd {
+	INJ_CMD_END   = 0x0, /* stop the FSM */
+	INJ_CMD_SEND  = 0x1, /* Send message on CCx */
+			     /* arg0: header arg1/2:payload index/count  */
+	INJ_CMD_WAVE  = 0x2, /* Send arbitrary waveform */
+			     /* stored at index arg1 of len arg0 */
+	INJ_CMD_HRST  = 0x3, /* Send Hard Reset on CCx */
+	INJ_CMD_WAIT  = 0x4, /* Wait for arg12 edges if arg12 != 0 */
+			     /* and timeout after arg0 ms */
+	INJ_CMD_GET   = 0x5, /* Get parameter arg1 (INJ_GET_x) at index arg0 */
+	INJ_CMD_SET   = 0x6, /* Set parameter arg1 (INJ_SET_x) with arg0 */
+	INJ_CMD_JUMP  = 0x8, /* Jump to index (as arg0) */
+	INJ_CMD_NOP   = 0xF, /* No-Operation */
+};
+
+enum inj_set {
+	INJ_SET_RESISTOR1  = 0, /* CC1 resistor as arg0 (INJ_RES_x) */
+	INJ_SET_RESISTOR2  = 1, /* CC2 resistor as arg0 (INJ_RES_x) */
+	INJ_SET_RECORD     = 2, /* Recording on/off */
+	INJ_SET_TX_SPEED   = 3, /* TX frequency is arg0 kHz */
+	INJ_SET_RX_THRESH  = 4, /* RX voltage threshold is arg0 mV */
+	INJ_SET_POLARITY   = 5, /* Polarity for other operations (INJ_POL_CC) */
+};
+
+enum inj_get {
+	INJ_GET_CC       = 0,  /* CC1/CC2 voltages in mV */
+	INJ_GET_VBUS     = 1,  /* VBUS voltage in mV and current in mA */
+	INJ_GET_VCONN    = 2, /* VCONN voltage in mV and current in mA */
+	INJ_GET_POLARITY = 3, /* Current polarity (INJ_POL_CC) */
+};
+
+enum inj_res {
+	INJ_RES_NONE  = 0,
+	INJ_RES_RA    = 1,
+	INJ_RES_RD    = 2,
+	INJ_RES_RPUSB = 3,
+	INJ_RES_RP1A5 = 4,
+	INJ_RES_RP3A0 = 5,
+};
+
+enum inj_pol {
+	INJ_POL_CC1 = 0,
+	INJ_POL_CC2 = 1,
+	INJ_POL_AUTO = 0xffff,
+};
+
+/* Number of words in the FSM command/data buffer  */
+#define INJ_CMD_COUNT 128
+
+#endif /* __INJECTOR_H */
diff --git a/board/twinkie/sniffer.c b/board/twinkie/sniffer.c
index 136cc1b01a..e6a0cff0a4 100644
--- a/board/twinkie/sniffer.c
+++ b/board/twinkie/sniffer.c
@@ -299,6 +299,45 @@ void sniffer_task(void)
 	}
 }
 
+int wait_packet(int pol, uint32_t min_edges, uint32_t timeout_us)
+{
+	stm32_dma_chan_t *chan = dma_get_channel(pol ? DMAC_TIM_RX2
+						     : DMAC_TIM_RX1);
+	uint32_t t0 = __hw_clock_source_read();
+	uint32_t c0 = chan->cndtr;
+	uint32_t t_gap = t0;
+	uint32_t c_gap = c0;
+	uint32_t total_edges = 0;
+
+	while (1) {
+		uint32_t t = __hw_clock_source_read();
+		uint32_t c = chan->cndtr;
+		if (t - t0 > timeout_us) /* Timeout */
+			break;
+		if (min_edges) { /* real packet detection */
+			int nb = (int)c_gap - (int)c;
+			if (nb < 0)
+				nb = RX_COUNT - nb;
+			if (nb > 3) { /* NOT IDLE */
+				t_gap = t;
+				c_gap = c;
+				total_edges += nb;
+			} else {
+				if ((t - t_gap) > 20 &&
+				    (total_edges - (t - t0)/256) >= min_edges)
+					/* real gap after the packet */
+					break;
+			}
+		}
+	}
+	return (__hw_clock_source_read() - t0 > timeout_us);
+}
+
+void recording_enable(uint8_t mask)
+{
+	/* TODO implement */
+}
+
 static int command_sniffer(int argc, char **argv)
 {
 	ccprintf("Seq number:%d Overflows: %d\n", seq, oflow);
diff --git a/board/twinkie/usb_pd_config.h b/board/twinkie/usb_pd_config.h
index e32b29fd67..8e5768a8c4 100644
--- a/board/twinkie/usb_pd_config.h
+++ b/board/twinkie/usb_pd_config.h
@@ -12,7 +12,11 @@
 
 /* Port and task configuration */
 #define PD_PORT_COUNT 1
+#ifdef HAS_TASK_PD /* PD message injector mode */
 #define PORT_TO_TASK_ID(port) TASK_ID_PD
+#else
+#define PORT_TO_TASK_ID(port) -1
+#endif
 #define TASK_ID_TO_PORT(id)   0
 
 /* Timer selection for baseband PD communication */
diff --git a/chip/stm32/usb_pd_phy.c b/chip/stm32/usb_pd_phy.c
index 784e294fd7..7079709e99 100644
--- a/chip/stm32/usb_pd_phy.c
+++ b/chip/stm32/usb_pd_phy.c
@@ -493,12 +493,6 @@ void pd_hw_init(int port)
 	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);
@@ -529,6 +523,12 @@ void pd_hw_init(int port)
 	/* Reload the pre-scaler and reset the counter */
 	phy->tim_tx->egr = 0x0001;
 #endif
+#ifndef CONFIG_USB_PD_TX_PHY_ONLY
+	/* 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;
 
 	/* --- set counter for RX timing : 2.4Mhz rate, free-running --- */
 	__hw_timer_enable_clock(TIM_CLOCK_PD_RX(port), 1);
@@ -600,6 +600,7 @@ void pd_hw_init(int port)
 	EXTI_XTSR |= EXTI_COMP_MASK(port);
 	STM32_EXTI_IMR |= EXTI_COMP_MASK(port);
 	task_enable_irq(IRQ_COMP);
+#endif /* CONFIG_USB_PD_TX_PHY_ONLY */
 
 	CPRINTS("USB PD initialized");
 }
diff --git a/common/usb_pd_protocol.c b/common/usb_pd_protocol.c
index 9f7b76670b..a160e61a11 100644
--- a/common/usb_pd_protocol.c
+++ b/common/usb_pd_protocol.c
@@ -394,15 +394,15 @@ static inline int encode_short(int port, int off, uint16_t val16)
 	return pd_write_sym(port, off, bmc4b5b[(val16 >> 12) & 0xF]);
 }
 
-static inline int encode_word(int port, int off, uint32_t val32)
+int encode_word(int port, int off, uint32_t val32)
 {
 	off = encode_short(port, off, (val32 >> 0) & 0xFFFF);
 	return encode_short(port, off, (val32 >> 16) & 0xFFFF);
 }
 
 /* prepare a 4b/5b-encoded PD message to send */
-static int prepare_message(int port, uint16_t header, uint8_t cnt,
-			   const uint32_t *data)
+int prepare_message(int port, uint16_t header, uint8_t cnt,
+		   const uint32_t *data)
 {
 	int off, i;
 	/* 64-bit preamble */
@@ -442,7 +442,7 @@ static int prepare_message(int port, uint16_t header, uint8_t cnt,
 static int analyze_rx(int port, uint32_t *payload);
 static void analyze_rx_bist(int port);
 
-static void send_hard_reset(int port)
+void send_hard_reset(int port)
 {
 	int off;
 
diff --git a/include/config.h b/include/config.h
index b3e753843d..c1f2b24198 100644
--- a/include/config.h
+++ b/include/config.h
@@ -1073,6 +1073,9 @@
 /* Include all USB Power Delivery modules */
 #undef CONFIG_USB_POWER_DELIVERY
 
+/* Alternative configuration keeping only the TX part of PHY */
+#undef CONFIG_USB_PD_TX_PHY_ONLY
+
 /* Default state of PD communication enabled flag */
 #define CONFIG_USB_PD_COMM_ENABLED 1
 
diff --git a/include/usb_pd.h b/include/usb_pd.h
index cb588102d0..d498570366 100644
--- a/include/usb_pd.h
+++ b/include/usb_pd.h
@@ -947,6 +947,29 @@ int pd_write_sym(int port, int bit_off, uint32_t val10);
 int pd_write_last_edge(int port, int bit_off);
 
 /**
+ * Do 4B5B encoding on a 32-bit word.
+ *
+ * @param port USB-C port number
+ * @param off current offset in bits inside the message
+ * @param val32 32-bit word value to encode
+ * @return new offset in the message in bits.
+ */
+int encode_word(int port, int off, uint32_t val32);
+
+/**
+ * Ensure that we have an edge after EOP and we end up at level 0,
+ * also fill the last byte.
+ *
+ * @param port USB-C port number
+ * @param header PD packet header
+ * @param cnt number of payload words
+ * @param data payload content
+ * @return length of the message in bits.
+ */
+int prepare_message(int port, uint16_t header, uint8_t cnt,
+		    const uint32_t *data);
+
+/**
  * Dump the current PD packet on the console for debug.
  *
  * @param port USB-C port number