Merge remote-tracking branch 'origin/dual-port'

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

7 files changed, 409 insertions, 92 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5d0a2e4..f0e853c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -18,6 +18,11 @@ add_executable(${PROJECT_NAME}
     FUSB302.c
     tcpm_driver.c
     vdmtool.c
+    usb_descriptors.c
+)
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+    ${CMAKE_CURRENT_LIST_DIR}
 )
 
 # Create map/bin/hex/uf2 files
@@ -27,9 +32,9 @@ pico_add_extra_outputs(${PROJECT_NAME})
 target_link_libraries(${PROJECT_NAME} 
     pico_stdlib
     hardware_i2c
+    pico_unique_id
+    tinyusb_board
+    tinyusb_device
 )
 
-# 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/README.txt b/README.txt
index 686cac2..a7ac2c3 100644
--- a/README.txt
+++ b/README.txt
@@ -4,6 +4,11 @@ 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.
 
+Otherwise, you can look at
+https://git.kernel.org/pub/scm/linux/kernel/git/maz/cs-hw.git
+for the hardware side of the project. Everything in this file
+assumes that the board is assembled and functionnal.
+
 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
@@ -29,12 +34,11 @@ 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.
+I've used versions 1.4 and 1.5 of the SDK. YMMV.
 
   git clone -b master https://github.com/raspberrypi/pico-sdk.git
 
-  export PICO_SDK_PATH=/the/path/to/pico-sdk
+  export PICO_SDK_PATH=/the/long/and/winding/road/to/pico-sdk
 
   cd pico-sdk
 
@@ -80,34 +84,54 @@ 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.
+doesn't). None of my cables labelled USB2.0 seem to work. I've had
+good results with cables designed to carry video signals, and USB3.1
+cables seem to do the trick.
 
 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.
+board's PD controller, and there is no provision to swap TX and RX.
+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.
+
+Also, there is only *ONE* USB-C port that is capable of serial
+output. The device will happily connect to it, and allow things like
+rebooting the Mac, but you won't get any serial output.
+
+Models I know of:
+- M1 mini 2020: USB-C port closest to the power supply
+- M1 Ultra 2022: USB-C port closest to the Ethernet port
+- M2 MacBook Air 2022: USB-C port closest to the MagSafe port
+- M2 Pro mini 2023: USB-C port closest to the Ethernet port (Oliver)
+
+No idea about other machines.
+
+Optionally, you can make use of the micro-USB connector that is on
+the other side of the board. It's main use it to allow interacting
+with the Asahi m1n1 firmware, such as tethered booting.
 
 ** 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:
+the Pico led blinking at the rate of twice a second, and a couple of
+/dev/ttyACM* being advertised 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
+  [708023.097129] usb 1-4: new full-speed USB device number 72 using xhci_hcd
+  [708023.265195] usb 1-4: New USB device found, idVendor=2e8a, idProduct=000a, bcdDevice= 1.00
+  [708023.265213] usb 1-4: New USB device strings: Mfr=1, Product=2, SerialNumber=3
+  [708023.265219] usb 1-4: Product: Central Scrutinizer
+  [708023.265223] usb 1-4: Manufacturer: AAAFNRAA
+  [708023.265228] usb 1-4: SerialNumber: E66164084319392A
+  [708023.273622] cdc_acm 1-4:1.0: ttyACM0: USB ACM device
+  [708023.278612] cdc_acm 1-4:1.2: ttyACM1: USB ACM device
 
-The board identifies itself as a Pico, not the Central Scrutinizer you
-were hoping for. Again, I'm lazy. Who cares?
+The board identifies itself as a Pico (as per VID/PID), and claims to
+be the Central Scrutinizer, as you were hoping for.
 
-Just run:
+Each of the two /dev/ttyACM* devices is a potential connection to a
+Mac. Assuming the likely case that you have a single USB-C port on
+your board, run:
 
   screen /dev/ttyACM0
 
@@ -124,7 +148,6 @@ and you should see something like:
   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)
@@ -134,7 +157,8 @@ and you should see something like:
   P0: IRQ=71 4 0
   P0: S: DFP_CONNECTED
   P0: >VDM serial -> SBU1/2
-  P0: IRQ=71 4 0
+  P0: IRQ=71 4 1
+  P0: <VDM RX SOP"DEBUG (5) [504f] 5ac8052 91340000 306 0 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
@@ -143,9 +167,14 @@ use any serial configuration you want on the Mac side as long as it is
 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.
 
+Replace "screen" with whatever you want to communicate with the
+device, be it conserver, minicom, cu, or even cat (there is no
+accounting for taste).
+
 Typing ^_? (Control-Underscore followed by a question mark) will lead
 to the follwing dump:
 
+  P0: Current port
   ^_    Escape character
   ^_ ^_ Raw ^_
   ^_ ^@ Send break
@@ -155,9 +184,9 @@ to the follwing dump:
   ^_ ^D Toggle debug
   ^_ ^M Send empty debug VDM
   ^_ ?  This message
-  P0: present
-  P1: absent
-  
+  P0: Port 0: present
+  P0: Port 1: present
+
 which is completely self explainatory, but let's expand on it anyway:
 
 - ^_ ^_ sends a raw ^_, just in case you really need it
@@ -184,6 +213,6 @@ which is completely self explainatory, but let's expand on it anyway:
 
 - ^_ ? prints the help message (duh).
 
-Finally, the P0:/P1: lines indicate which I2C/UART combination the
+Finally, the Port 0:/1: lines indicate which I2C/UART combinations 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.
+Pico, but it is very unlikely you have built such a configuration.
diff --git a/m1-pd-bmc.h b/m1-pd-bmc.h
index 3ccf5a0..d350c80 100644
--- a/m1-pd-bmc.h
+++ b/m1-pd-bmc.h
@@ -7,6 +7,7 @@
 
 #include "hardware/gpio.h"
 #include "hardware/i2c.h"
+#include "tusb.h"
 
 struct gpio_pin_config {
 	uint16_t		pin;
@@ -40,3 +41,18 @@ 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);
+
+void usb_tx_bytes(int32_t port, const char *ptr, int len);
+void usb_tx_str(int32_t port, char *ptr);
+int32_t usb_rx_byte(int32_t port);
+
+#define PRINTF_SIZE	256
+
+#define __printf(__p, __f, ...)	do {					\
+		char __str[PRINTF_SIZE];				\
+		snprintf(__str, PRINTF_SIZE, __f, ##__VA_ARGS__);	\
+		usb_tx_str(__p, __str);					\
+		tud_task();						\
+	} while(0)
+
+#define ARRAY_SIZE(arr)	(sizeof(arr) / sizeof((arr)[0]))
diff --git a/start.c b/start.c
index fecd022..0d494b1 100644
--- a/start.c
+++ b/start.c
@@ -3,6 +3,8 @@
 #include <stddef.h>
 #include <stdio.h>
 
+#include "bsp/board.h"
+#include "tusb.h"
 #include "m1-pd-bmc.h"
 #include "FUSB302.h"
 
@@ -75,12 +77,13 @@ static const struct gpio_pin_config m1_pd_bmc_pin_config1[] = {
 	},
 };
 
-#define ARRAY_SIZE(arr)	(sizeof(arr) / sizeof((arr)[0]))
-
-static void __not_in_flash_func(uart_irq_fn)(const struct hw_context *hw)
+static void __not_in_flash_func(uart_irq_fn)(int port,
+					     const struct hw_context *hw)
 {
-	while (uart_is_readable(hw->uart))
-		putchar_raw(uart_getc(hw->uart));
+	while (uart_is_readable(hw->uart)) {
+		char c = uart_getc(hw->uart);
+		usb_tx_bytes(port, &c, 1);
+	}
 }
 
 static void uart0_irq_fn(void);
@@ -108,12 +111,12 @@ static const struct hw_context hw1 = {
 
 static void __not_in_flash_func(uart0_irq_fn)(void)
 {
-	uart_irq_fn(&hw0);
+	uart_irq_fn(0, &hw0);
 }
 
 static void __not_in_flash_func(uart1_irq_fn)(void)
 {
-	uart_irq_fn(&hw1);
+	uart_irq_fn(1, &hw1);
 }
 
 static void init_system(const struct hw_context *hw)
@@ -154,30 +157,93 @@ static void m1_pd_bmc_system_init(const struct hw_context *hw)
 		m1_pd_bmc_gpio_setup_one(&hw->pins[i]);
 }
 
+void usb_tx_bytes(int32_t port, const char *ptr, int len)
+{
+        if (!tud_cdc_n_connected(port))
+		return;
+
+	while (len > 0) {
+		size_t available = tud_cdc_n_write_available(port);
+
+		if (!available) {
+			tud_task();
+		} else {
+			size_t send = MIN(len, available);
+			size_t sent = tud_cdc_n_write(port, ptr, send);
+
+			ptr += sent;
+			len -= sent;
+		}
+
+		tud_cdc_n_write_flush(port);
+        }
+}
+
+void usb_tx_str(int32_t port, char *str)
+{
+	do {
+		char *cursor = str;
+
+		while (*cursor && *cursor != '\n')
+			cursor++;
+
+		usb_tx_bytes(port, str, cursor - str);
+
+		if (!*cursor)
+			return;
+
+		usb_tx_bytes(port, "\n\r", 2);
+
+		str = cursor + 1;
+	} while (*str);
+}
+
+int32_t usb_rx_byte(int32_t port)
+{
+	uint8_t c;
+
+	if (!tud_cdc_n_connected(port) || !tud_cdc_n_available(port))
+		return -1;
+
+	tud_cdc_n_read(port, &c, 1);
+	return c;
+}
+
 int main(void)
 {
 	bool success;
+	int port;
 
 	success = set_sys_clock_khz(250000, false);
 
+	board_init();
+	tusb_init();
 	stdio_init_all();
 
 	m1_pd_bmc_system_init(&hw0);
 	m1_pd_bmc_system_init(&hw1);
 
-	while (!stdio_usb_connected()) {
+	do {
 		static bool state = false;
+		static int cnt = 0;
 
+		tud_task();
 		gpio_put(hw0.pins[LED_G].pin, state);
-		sleep_ms(250);
-		state = !state;
-	}
+		sleep_ms(1);
+		cnt++;
+		cnt %= 256;
+		if (!cnt)
+			state = !state;
+	} while (!tud_cdc_n_connected(0) && !tud_cdc_n_connected(1));
+
+	port = !tud_cdc_n_connected(0);
+
+	__printf(port, "This is the Central Scrutinizer\n");
+	__printf(port, "Control character is ^_\n");
+	__printf(port, "Press ^_ + ? for help\n");
 
-	printf("This is the Central Scrutinizer\n"
-		"Control character is ^_\n"
-		"Press ^_ + ? for help\n");
 	if (!success)
-		printf("WARNING: Nominal frequency NOT reached\n");
+		__printf(port, "WARNING: Nominal frequency NOT reached\n");
 
 	m1_pd_bmc_fusb_setup(0, &hw0);
 	m1_pd_bmc_fusb_setup(1, &hw1);
diff --git a/tusb_config.h b/tusb_config.h
new file mode 100644
index 0000000..a278278
--- /dev/null
+++ b/tusb_config.h
@@ -0,0 +1,26 @@
+#ifndef _TUSB_CONFIG_H_
+#define _TUSB_CONFIG_H_
+
+#define CFG_TUSB_RHPORT0_MODE   OPT_MODE_DEVICE
+
+#define CFG_TUD_EP_MAX          5
+
+#define CFG_TUD_CDC             2
+
+#define CFG_TUD_CDC_EP_BUFSIZE  512
+#define CFG_TUD_CDC_RX_BUFSIZE  512
+#define CFG_TUD_CDC_TX_BUFSIZE  512
+
+#define CFG_TUD_WEB             0
+#define CFG_TUD_ECM_RNDIS       0
+#define CFG_TUD_MSC             0
+#define CFG_TUD_HID             0
+#define CFG_TUD_MIDI            0
+#define CFG_TUD_AUDIO           0
+#define CFG_TUD_BTH             0
+#define CFG_TUD_TMC             0
+#define CFG_TUD_GUD             0
+#define CFG_TUD_VENDOR          0
+
+#endif
+
diff --git a/usb_descriptors.c b/usb_descriptors.c
new file mode 100644
index 0000000..a084ae6
--- /dev/null
+++ b/usb_descriptors.c
@@ -0,0 +1,152 @@
+#include "tusb.h"
+#include "pico/unique_id.h"
+#include "m1-pd-bmc.h"
+
+/* Still a RPi Pico, but let's be creative anyway */
+#define USBD_VID                (0x2e8a)
+#define USBD_PID                (0x000a)
+
+enum {
+	USBD_STR_LANGUAGE,          // 0
+	USBD_STR_MANUFACTURER,      // 1
+	USBD_STR_PRODUCT,           // 2
+	USBD_STR_SERIAL_NUMBER,     // 3
+	USBD_STR_CDC_0_NAME,        // 4
+	USBD_STR_CDC_1_NAME,        // 5
+	USBD_STR_LAST,
+};
+
+static const tusb_desc_device_t usbd_desc_device = {
+	.bLength                    = sizeof(tusb_desc_device_t),
+	.bDescriptorType            = TUSB_DESC_DEVICE,
+	.bcdUSB                     = 0x0200,
+	.bDeviceClass               = TUSB_CLASS_MISC,
+	.bDeviceSubClass            = MISC_SUBCLASS_COMMON,
+	.bDeviceProtocol            = MISC_PROTOCOL_IAD,
+	.bMaxPacketSize0            = CFG_TUD_ENDPOINT0_SIZE,
+	.idVendor                   = USBD_VID,
+	.idProduct                  = USBD_PID,
+	.bcdDevice                  = 0x0100,
+	.iManufacturer              = USBD_STR_MANUFACTURER,
+	.iProduct                   = USBD_STR_PRODUCT,
+	.iSerialNumber              = USBD_STR_SERIAL_NUMBER,
+	.bNumConfigurations         = 1,
+};
+
+#define EPNUM_CDC_0_CMD         (0x81)
+#define EPNUM_CDC_0_DATA        (0x82)
+
+#define EPNUM_CDC_1_CMD         (0x83)
+#define EPNUM_CDC_1_DATA        (0x84)
+
+#define USBD_CDC_CMD_SIZE       (64)
+#define USBD_CDC_DATA_SIZE      (64)
+
+#define USBD_MAX_POWER_MA       (250)
+
+#define USBD_DESC_LEN           (TUD_CONFIG_DESC_LEN + \
+				 TUD_CDC_DESC_LEN * CFG_TUD_CDC)
+
+enum {
+	ITF_NUM_CDC_0,
+	ITF_NUM_CDC_0_DATA,
+	ITF_NUM_CDC_1,
+	ITF_NUM_CDC_1_DATA,
+	ITF_NUM_TOTAL,
+};
+
+static const uint8_t usbd_desc_cfg[USBD_DESC_LEN] = {
+
+	TUD_CONFIG_DESCRIPTOR(1, ITF_NUM_TOTAL,
+			      USBD_STR_LANGUAGE,
+			      USBD_DESC_LEN,
+			      TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP,
+			      USBD_MAX_POWER_MA),
+
+	TUD_CDC_DESCRIPTOR(ITF_NUM_CDC_0,
+			   USBD_STR_CDC_0_NAME,
+			   EPNUM_CDC_0_CMD,
+			   USBD_CDC_CMD_SIZE,
+			   EPNUM_CDC_0_DATA & 0x7F,
+			   EPNUM_CDC_0_DATA,
+			   USBD_CDC_DATA_SIZE),
+
+	TUD_CDC_DESCRIPTOR(ITF_NUM_CDC_1,
+			   USBD_STR_CDC_1_NAME,
+			   EPNUM_CDC_1_CMD,
+			   USBD_CDC_CMD_SIZE,
+			   EPNUM_CDC_1_DATA & 0x7F,
+			   EPNUM_CDC_1_DATA,
+			   USBD_CDC_DATA_SIZE),
+};
+
+const uint8_t *tud_descriptor_device_cb(void)
+{
+	return (const uint8_t *)&usbd_desc_device;
+}
+
+const uint8_t *tud_descriptor_configuration_cb(uint8_t index)
+{
+	(void)index;
+	return usbd_desc_cfg;
+}
+
+#define DESC_STR_MAX_LENGTH 20
+#define USB_DESC_STRLEN(l)	((TUSB_DESC_STRING << 8) | ((l + 1) * 2))
+
+static void str8_to_str16(const char *str, uint16_t *str16)
+{
+	int i;
+
+	for (i = 0; i < DESC_STR_MAX_LENGTH - 1 && str[i]; i++)
+		str16[i + 1] = str[i];
+
+	str16[0] = USB_DESC_STRLEN(i);
+}
+
+const uint16_t *tud_descriptor_string_cb(uint8_t index, uint16_t langid)
+{
+	static uint16_t desc_str[DESC_STR_MAX_LENGTH];
+
+	if (index >= USBD_STR_LAST)
+		return NULL;
+
+	switch (index) {
+	case USBD_STR_LANGUAGE:
+		desc_str[0] = USB_DESC_STRLEN(1);
+		desc_str[1] = 0x0409; // English
+		break;
+
+	case USBD_STR_SERIAL_NUMBER: {
+		char str[DESC_STR_MAX_LENGTH] = {};
+		pico_unique_board_id_t id;
+
+		pico_get_unique_board_id(&id);
+		snprintf(str, DESC_STR_MAX_LENGTH,
+			 "%02X%02X%02X%02X%02X%02X%02X%02X",
+			 id.id[0], id.id[1], id.id[2], id.id[3],
+			 id.id[4], id.id[5], id.id[6], id.id[7]);
+
+		str8_to_str16(str, desc_str);
+		break;
+	}
+
+	case USBD_STR_MANUFACTURER:
+		str8_to_str16("AAAFNRAA", desc_str);
+		break;
+
+	case USBD_STR_PRODUCT:
+		str8_to_str16("Central Scrutinizer", desc_str);
+		break;
+
+	case USBD_STR_CDC_0_NAME:
+		str8_to_str16("Port-0", desc_str);
+		break;
+
+	case USBD_STR_CDC_1_NAME:
+		str8_to_str16("Port-1", desc_str);
+		break;
+	}
+
+	return desc_str;
+}
diff --git a/vdmtool.c b/vdmtool.c
index 4f7916d..f6a01a9 100644
--- a/vdmtool.c
+++ b/vdmtool.c
@@ -43,15 +43,20 @@ static struct vdm_context vdm_contexts[CONFIG_USB_PD_PORT_COUNT];
 #define HIGH true
 #define LOW false
 
-#define dprintf(cxt, ...)	do {					\
-		if (cxt->verbose)					\
-			printf(__VA_ARGS__);				\
-		} while(0)
+#define cprintf_cont(cxt, str, ...)	do {				\
+		__printf(PORT(cxt), str, ##__VA_ARGS__);		\
+	} while(0)
 
 #define cprintf(cxt, str, ...)	do {					\
-		printf("P%ld: " str, PORT(cxt), ##__VA_ARGS__);		\
+		cprintf_cont(cxt,					\
+			     "P%ld: " str, PORT(cxt), ##__VA_ARGS__);	\
 	} while(0)
 
+#define dprintf(cxt, ...)	do {					\
+		if (cxt->verbose)					\
+			cprintf(cxt, ##__VA_ARGS__);			\
+		} while(0)
+
 #define STATE(cxt, x)	do {						\
 		cxt->state = STATE_##x;					\
 		cprintf(cxt, "S: " #x "\n");				\
@@ -176,36 +181,36 @@ static void send_source_cap(struct vdm_context *cxt)
 	cxt->source_cap_timer = 0;
 }
 
-static void dump_msg(enum fusb302_rxfifo_tokens sop, int16_t hdr, uint32_t * msg)
+static void dump_msg(struct vdm_context *cxt,
+		     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 (");
+		cprintf_cont(cxt, "RX SOP (");
 		break;
 	case fusb302_TKN_SOP1:
-		printf("RX SOP' (");
+		cprintf_cont(cxt, "RX SOP' (");
 		break;
 	case fusb302_TKN_SOP2:
-		printf("RX SOP\" (");
+		cprintf_cont(cxt, "RX SOP\" (");
 		break;
 	case fusb302_TKN_SOP1DB:
-		printf("RX SOP'DEBUG (");
+		cprintf_cont(cxt, "RX SOP'DEBUG (");
 		break;
 	case fusb302_TKN_SOP2DB:
-		printf("RX SOP\"DEBUG (");
+		cprintf_cont(cxt, "RX SOP\"DEBUG (");
 		break;
 	default:
-		printf("RX ? (");
+		cprintf_cont(cxt, "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");
+	cprintf_cont(cxt, "%d) [%x]", len, hdr);
+	for (int16_t i = 0; i < PD_HEADER_CNT(hdr); i++)
+		cprintf_cont(cxt, " %x", msg[i]);
+
+	cprintf_cont(cxt, "\n");
 }
 
 static void handle_discover_identity(struct vdm_context *cxt)
@@ -269,7 +274,7 @@ static void handle_vdm(struct vdm_context *cxt, enum fusb302_rxfifo_tokens sop,
 		break;
 	default:
 		cprintf(cxt, "<VDM ");
-		dump_msg(sop, hdr, msg);
+		dump_msg(cxt, sop, hdr, msg);
 		break;
 	}
 }
@@ -295,7 +300,7 @@ static void handle_msg(struct vdm_context *cxt, enum fusb302_rxfifo_tokens sop,
 			break;
 		default:
 			cprintf(cxt, "<UNK DATA ");
-			dump_msg(sop, hdr, msg);
+			dump_msg(cxt, sop, hdr, msg);
 			break;
 		}
 	} else {
@@ -323,7 +328,7 @@ static void handle_msg(struct vdm_context *cxt, enum fusb302_rxfifo_tokens sop,
 			break;
 		default:
 			cprintf(cxt, "<UNK CTL ");
-			dump_msg(sop, hdr, msg);
+			dump_msg(cxt, sop, hdr, msg);
 			break;
 		}
 	}
@@ -370,11 +375,11 @@ static void handle_irq(struct vdm_context *cxt)
 	if (irq & TCPC_REG_INTERRUPT_VBUSOK) {
 		cprintf(cxt, "IRQ: VBUSOK (VBUS=");
 		if (fusb302_tcpm_get_vbus_level(PORT(cxt))) {
-			cprintf(cxt, "ON)\n");
+			cprintf_cont(cxt, "ON)\n");
 			send_source_cap(cxt);
 			debug_poke(cxt);
 		} else {
-			cprintf(cxt, "OFF)\n");
+			cprintf_cont(cxt, "OFF)\n");
 			evt_disconnect(cxt);
 		}
 	}
@@ -415,7 +420,7 @@ 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");
+	cprintf(cxt, ">VDM SET ACTION reboot\n");
 }
 
 static void serial_out(struct vdm_context *cxt, char c)
@@ -423,28 +428,30 @@ static void serial_out(struct vdm_context *cxt, char c)
 	uart_putc_raw(UART(cxt), c);
 }
 
-static void help(void)
+static void help(struct vdm_context *cxt)
 {
-	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");
+	cprintf(cxt, "Current port\n"
+		"^_    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",
+		cprintf(cxt, "Port %d: %s\n",
+			PORT(&vdm_contexts[i]),
 			vdm_contexts[i].hw ? "present" : "absent");
 }
 
 static bool serial_handler(struct vdm_context *cxt)
 {
 	bool uart_active = false;
-	int c;
+	int32_t c;
 
-	while ((c = getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) {
+	while ((c = usb_rx_byte(PORT(cxt))) != -1) {
 		uart_active = true;
 
 		if ((!cxt->vdm_escape && c != 0x1f)) {
@@ -482,7 +489,7 @@ static bool serial_handler(struct vdm_context *cxt)
 			debug_poke(cxt);
 			break;
 		case '?':
-			help();
+			help(cxt);
 			break;
 		}
 
@@ -528,8 +535,7 @@ static void state_machine(struct vdm_context *cxt)
 		break;
 	}
 	default:{
-		cprintf(cxt, "Invalid state %d", cxt->state);
-		cprintf(cxt, "\n");
+		cprintf(cxt, "Invalid state %d\n", cxt->state);
 	}
 	}
 	if (cxt->state != STATE_DISCONNECTED) {
@@ -641,6 +647,12 @@ static bool m1_pd_bmc_run_one(struct vdm_context *cxt)
 	return serial_handler(cxt) || cxt->pending;
 }
 
+#define for_each_cxt(___c)						\
+	for (struct vdm_context *___c = &vdm_contexts[0];		\
+	     (___c - vdm_contexts) < CONFIG_USB_PD_PORT_COUNT;		\
+	     ___c++)							\
+		if (___c->hw)
+
 void m1_pd_bmc_run(void)
 {
 	int i;
@@ -648,12 +660,23 @@ void m1_pd_bmc_run(void)
 	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]);
+		for_each_cxt(cxt) {
+			gpio_put(PIN(cxt, LED_G), HIGH);
+			busy |= m1_pd_bmc_run_one(cxt);
+			irq_set_enabled(cxt->hw->uart_irq, false);
 		}
 
-		if (!busy)
-			__wfi();
+		tud_task();
+
+		for_each_cxt(cxt)
+			irq_set_enabled(cxt->hw->uart_irq, true);
+
+		if (busy)
+			continue;
+
+		for_each_cxt(cxt)
+			gpio_put(PIN(cxt, LED_G), LOW);
+
+		__wfe();
 	}
 }