Add CDC stdio driver and logging + panic handlers

8 files changed, 416 insertions, 234 deletions

diff --git a/src/board.h b/src/board.h
new file mode 100644
index 0000000..38775b7
--- /dev/null
+++ b/src/board.h
@@ -0,0 +1,7 @@
+#pragma once
+
+#include "neopix.h"
+
+#define ONBOARD_LED_PIN 25
+
+extern struct neopix onboard_led;
diff --git a/src/main.c b/src/main.c
index d73c530..e679518 100644
--- a/src/main.c
+++ b/src/main.c
@@ -1,84 +1,132 @@
+#include "board.h"
+#include "class/cdc/cdc_device.h"
+#include "neopix.h"
+#include "util.h"
+
+#include "pico/stdio/driver.h"
+#include "pico/stdlib.h"
+#include "bsp/board.h"
+#include "pico/time.h"
+#include "tusb.h"
+
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
 #include <sys/types.h>
 
-#include "bsp/board.h"
-#include "tusb.h"
-#include "neopix.h"
-
-/* Blink pattern
- * - 250 ms  : device not mounted
- * - 1000 ms : device mounted
- * - 2500 ms : device is suspended
- */
-enum	{
+enum {
 	BLINK_NOT_MOUNTED = 250,
 	BLINK_MOUNTED = 1000,
 	BLINK_SUSPENDED = 2500,
 };
 
-static uint32_t blink_interval_ms = BLINK_NOT_MOUNTED;
-
-static struct neopix onboard_led;
+void stub_stdio_write(const char *buf, int len);
+void stub_stdio_flush(void);
+int stub_stdio_read(char *buf, int len);
 
 void led_blinking_task(void);
+void cdc_echo_task(void);
+void print_task(void);
+
+static struct stdio_driver usb_stdio = {
+	.out_chars = stub_stdio_write,
+	.out_flush = stub_stdio_flush,
+	.in_chars = stub_stdio_read,
+	.next = NULL
+};
+
+static uint32_t blink_interval_ms = BLINK_NOT_MOUNTED;
+
+struct neopix onboard_led;
 
-int main(void)
+int
+main(void)
 {
 	board_init();
 
+	(void) usb_stdio;
+	stdio_set_driver_enabled(&usb_stdio, true);
+	stdio_init_all();
+
 	neopix_init(&onboard_led, pio0, 0, 25);
 
 	tud_init(BOARD_TUD_RHPORT);
 
-	while (1) {
+	ASSERT(1 == 0);
+
+	while (!tud_cdc_connected())
+		tud_task();
+
+	while (true) {
 		tud_task();
 		led_blinking_task();
+		//cdc_echo_task();
+		print_task();
 	}
 
 	return 0;
 }
 
-//--------------------------------------------------------------------+
-// Device callbacks
-//--------------------------------------------------------------------+
+void
+stub_stdio_write(const char *buf, int len)
+{
+	tud_cdc_write(buf, (uint32_t) len);
+}
 
-// Invoked when device is mounted
-void tud_mount_cb(void)
+void
+stub_stdio_flush(void)
+{
+	tud_cdc_write_flush();
+}
+
+int
+stub_stdio_read(char *buf, int len)
+{
+	return (int) tud_cdc_read(buf, (uint32_t) len);
+}
+
+void
+tud_mount_cb(void)
 {
 	blink_interval_ms = BLINK_MOUNTED;
 }
 
-// Invoked when device is unmounted
-void tud_umount_cb(void)
+void
+tud_umount_cb(void)
 {
 	blink_interval_ms = BLINK_NOT_MOUNTED;
 }
 
-// Invoked when usb bus is suspended
-// remote_wakeup_en : if host allow us	to perform remote wakeup
-// Within 7ms, device must draw an average of current less than 2.5 mA from bus
-void tud_suspend_cb(bool remote_wakeup_en)
+void
+tud_suspend_cb(bool remote_wakeup_en)
 {
-	(void) remote_wakeup_en;
+	(void) remote_wakeup_en; /* host allows remote wakeup */
 	blink_interval_ms = BLINK_SUSPENDED;
 }
 
-// Invoked when usb bus is resumed
-void tud_resume_cb(void)
+void
+tud_resume_cb(void)
 {
 	blink_interval_ms = BLINK_MOUNTED;
 }
 
-//--------------------------------------------------------------------+
-// USB HID
-//--------------------------------------------------------------------+
+void
+tud_cdc_line_state_cb(uint8_t itf, bool dtr, bool rts)
+{
+	(void) itf;
+	(void) rts;
+	(void) dtr; /* TODO: show terminal connection with LED? */
+}
 
-// Invoked when received GET_REPORT control request
-// Application must fill buffer report's content and return its length.
-// Return zero will cause the stack to STALL request
-uint16_t tud_hid_get_report_cb(uint8_t itf, uint8_t report_id,
+void
+tud_cdc_rx_cb(uint8_t itf)
+{
+	(void) itf;
+}
+
+/* Invoked on GET_REPORT */
+uint16_t
+tud_hid_get_report_cb(uint8_t itf, uint8_t report_id,
 	hid_report_type_t report_type, uint8_t* buffer, uint16_t reqlen)
 {
 	(void) itf;
@@ -90,9 +138,9 @@ uint16_t tud_hid_get_report_cb(uint8_t itf, uint8_t report_id,
 	return 0;
 }
 
-// Invoked when received SET_REPORT control request or
-// received data on OUT endpoint ( Report ID = 0, Type = 0 )
-void tud_hid_set_report_cb(uint8_t itf, uint8_t report_id,
+/* Invoked on SET_REPORT or receive on OUT with (Report ID = 0, Type = 0) */
+void
+tud_hid_set_report_cb(uint8_t itf, uint8_t report_id,
 	hid_report_type_t report_type, uint8_t const* buffer, uint16_t bufsize)
 {
 	(void) itf;
@@ -116,3 +164,31 @@ led_blinking_task(void)
 	start_ms += blink_interval_ms;
 	state ^= true;
 }
+
+void
+cdc_echo_task(void)
+{
+	char buf[64];
+	uint32_t count;
+
+	if (tud_cdc_available()) {
+		count = tud_cdc_read(buf, sizeof(buf));
+
+		tud_cdc_write(buf, count);
+		tud_cdc_write_flush();
+	}
+}
+
+void
+print_task(void)
+{
+	static uint32_t start_ms = 0;
+
+	if (!tud_cdc_available())
+		return;
+
+	if (board_millis() - start_ms < 1000)
+		return;
+
+	start_ms += 1000;
+}
diff --git a/src/neopix.c b/src/neopix.c
index ddc78a3..107f354 100644
--- a/src/neopix.c
+++ b/src/neopix.c
@@ -14,6 +14,7 @@ neopix_init(struct neopix *pix, PIO pio, uint sm, uint pin)
 	offset = pio_add_program(pix->pio, &ws2812_program);
 	ws2812_program_init(pix->pio, pix->sm, offset,
 		pix->pin, 800000, false);
+	pix->init = true;
 }
 
 void
diff --git a/src/neopix.h b/src/neopix.h
index 8b01038..5ef3c4b 100644
--- a/src/neopix.h
+++ b/src/neopix.h
@@ -1,10 +1,15 @@
+#pragma once
+
 #include "hardware/pio.h"
 #include "hardware/gpio.h"
 
+#include <stdbool.h>
+
 struct neopix {
 	PIO pio;
 	uint sm;
 	uint pin;
+	bool init;
 };
 
 void neopix_init(struct neopix *pix, PIO pio, uint sm, uint pin);
diff --git a/src/tusb_config.h b/src/tusb_config.h
index 98143ac..cd251d6 100644
--- a/src/tusb_config.h
+++ b/src/tusb_config.h
@@ -1,54 +1,17 @@
-/*
- * The MIT License (MIT)
- *
- * Copyright (c) 2019 Ha Thach (tinyusb.org)
- *
- * 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.
- *
- */
-
-#ifndef _TUSB_CONFIG_H_
-#define _TUSB_CONFIG_H_
+#pragma once
 
 #ifdef __cplusplus
- extern "C" {
+extern "C" {
 #endif
 
-//--------------------------------------------------------------------+
-// Board Specific Configuration
-//--------------------------------------------------------------------+
-
-// RHPort number used for device can be defined by board.mk, default to port 0
 #ifndef BOARD_TUD_RHPORT
 #define BOARD_TUD_RHPORT      0
 #endif
 
-// RHPort max operational speed can defined by board.mk
 #ifndef BOARD_TUD_MAX_SPEED
 #define BOARD_TUD_MAX_SPEED   OPT_MODE_DEFAULT_SPEED
 #endif
 
-//--------------------------------------------------------------------
-// COMMON CONFIGURATION
-//--------------------------------------------------------------------
-
-// defined by compiler flags for flexibility
 #ifndef CFG_TUSB_MCU
 #error CFG_TUSB_MCU must be defined
 #endif
@@ -61,19 +24,10 @@
 #define CFG_TUSB_DEBUG        0
 #endif
 
-// Enable Device stack
 #define CFG_TUD_ENABLED       1
 
-// Default is max speed that hardware controller could support with on-chip PHY
 #define CFG_TUD_MAX_SPEED     BOARD_TUD_MAX_SPEED
 
-/* USB DMA on some MCUs can only access a specific SRAM region with restriction on alignment.
- * Tinyusb use follows macros to declare transferring memory so that they can be put
- * into those specific section.
- * e.g
- * - CFG_TUSB_MEM SECTION : __attribute__ (( section(".usb_ram") ))
- * - CFG_TUSB_MEM_ALIGN   : __attribute__ ((aligned(4)))
- */
 #ifndef CFG_TUSB_MEM_SECTION
 #define CFG_TUSB_MEM_SECTION
 #endif
@@ -82,26 +36,20 @@
 #define CFG_TUSB_MEM_ALIGN        __attribute__ ((aligned(4)))
 #endif
 
-//--------------------------------------------------------------------
-// DEVICE CONFIGURATION
-//--------------------------------------------------------------------
-
 #ifndef CFG_TUD_ENDPOINT0_SIZE
 #define CFG_TUD_ENDPOINT0_SIZE    64
 #endif
 
-//------------- CLASS -------------//
-#define CFG_TUD_CDC               0
+#define CFG_TUD_CDC               1
 #define CFG_TUD_MSC               0
 #define CFG_TUD_HID               1
 #define CFG_TUD_MIDI              0
 #define CFG_TUD_VENDOR            0
 
-// HID buffer size Should be sufficient to hold ID (if any) + Data
-#define CFG_TUD_HID_EP_BUFSIZE    64
+#define CFG_TUD_CDC_EP_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
+#define CFG_TUD_CDC_RX_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
+#define CFG_TUD_CDC_TX_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
 
 #ifdef __cplusplus
- }
+}
 #endif
-
-#endif /* _TUSB_CONFIG_H_ */
diff --git a/src/usb_descriptors.c b/src/usb_descriptors.c
index c2b4792..8372c2a 100644
--- a/src/usb_descriptors.c
+++ b/src/usb_descriptors.c
@@ -1,170 +1,205 @@
-/* 
- * The MIT License (MIT)
- *
- * Copyright (c) 2019 Ha Thach (tinyusb.org)
- *
- * 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.
- *
- */
-
 #include "tusb.h"
+#include "tusb_types.h"
 
-/* A combination of interfaces must have a unique product id, since PC will save device driver after the first plug.
- * Same VID/PID with different interface e.g MSC (first), then CDC (later) will possibly cause system error on PC.
- *
- * Auto ProductID layout's Bitmap:
- *   [MSB]         HID | MSC | CDC          [LSB]
- */
-#define _PID_MAP(itf, n)  ( (CFG_TUD_##itf) << (n) )
-#define USB_PID           (0x4000 | _PID_MAP(CDC, 0) | _PID_MAP(MSC, 1) | _PID_MAP(HID, 2) | \
-                           _PID_MAP(MIDI, 3) | _PID_MAP(VENDOR, 4) )
-
-//--------------------------------------------------------------------+
-// Device Descriptors
-//--------------------------------------------------------------------+
-tusb_desc_device_t const desc_device =
-{
-    .bLength            = sizeof(tusb_desc_device_t),
-    .bDescriptorType    = TUSB_DESC_DEVICE,
-    .bcdUSB             = 0x0200,
-    .bDeviceClass       = 0x00,
-    .bDeviceSubClass    = 0x00,
-    .bDeviceProtocol    = 0x00,
-    .bMaxPacketSize0    = CFG_TUD_ENDPOINT0_SIZE,
-
-    .idVendor           = 0xCafe,
-    .idProduct          = USB_PID,
-    .bcdDevice          = 0x0100,
-
-    .iManufacturer      = 0x01,
-    .iProduct           = 0x02,
-    .iSerialNumber      = 0x03,
-
-    .bNumConfigurations = 0x01
+#define ARRLEN(x) (sizeof(x) / sizeof((x)[0]))
+
+/* same VID/PID with difference interface can cause issues! */
+
+#define _PID_MAP(itf, n) ((CFG_TUD_##itf) << (n))
+#define USB_PID (0x4000 | _PID_MAP(CDC, 0) | _PID_MAP(MSC, 1) \
+	 | _PID_MAP(HID, 2) | _PID_MAP(MIDI, 3) | _PID_MAP(VENDOR, 4))
+
+#define USB_VID 0xC0FE
+#define USB_BCD 0x0200
+
+#define CONFIG_TOTAL_LEN \
+	(TUD_CONFIG_DESC_LEN + TUD_CDC_DESC_LEN + TUD_HID_INOUT_DESC_LEN)
+
+#define EPNUM_HID 0x01
+
+/* MCU-specific! */
+#define EPNUM_CDC_NOTIF   0x81
+#define EPNUM_CDC_OUT     0x02
+#define EPNUM_CDC_IN      0x82
+#define EPNUM_MSC_OUT     0x03
+#define EPNUM_MSC_IN      0x83
+
+enum {
+	ITF_NUM_CDC,
+	ITF_NUM_CTC_DATA,
+	ITF_NUM_HID,
+	ITF_NUM_TOTAL
 };
 
-// Invoked when received GET DEVICE DESCRIPTOR
-// Application return pointer to descriptor
-uint8_t const * tud_descriptor_device_cb(void)
-{
-  return (uint8_t const *) &desc_device;
-}
+tusb_desc_device_t const desc_device = {
+	.bLength            = sizeof(tusb_desc_device_t),
+	.bDescriptorType    = TUSB_DESC_DEVICE,
+	.bcdUSB             = USB_BCD,
 
-//--------------------------------------------------------------------+
-// HID Report Descriptor
-//--------------------------------------------------------------------+
+	.bDeviceClass       = TUSB_CLASS_MISC,
+	.bDeviceSubClass    = MISC_SUBCLASS_COMMON,
+	.bDeviceProtocol    = MISC_PROTOCOL_IAD,
 
-uint8_t const desc_hid_report[] =
-{
-  TUD_HID_REPORT_DESC_GENERIC_INOUT(CFG_TUD_HID_EP_BUFSIZE)
+	.bMaxPacketSize0    = CFG_TUD_ENDPOINT0_SIZE,
+
+	.idVendor           = USB_VID,
+	.idProduct          = USB_PID,
+	.bcdDevice          = 0x0100,
+
+	.iManufacturer      = 0x01,
+	.iProduct           = 0x02,
+	.iSerialNumber      = 0x03,
+
+	.bNumConfigurations = 0x01
 };
 
-// Invoked when received GET HID REPORT DESCRIPTOR
-// Application return pointer to descriptor
-// Descriptor contents must exist long enough for transfer to complete
-uint8_t const * tud_hid_descriptor_report_cb(uint8_t itf)
-{
-  (void) itf;
-  return desc_hid_report;
-}
+uint8_t const desc_hid_report[] = {
+	TUD_HID_REPORT_DESC_GENERIC_INOUT(CFG_TUD_HID_EP_BUFSIZE)
+};
 
-//--------------------------------------------------------------------+
-// Configuration Descriptor
-//--------------------------------------------------------------------+
+uint8_t const desc_fs_configuration[] = {
+	/* Config number, interface count, string index,
+	 * total length, attribute, power in mA */
+	TUD_CONFIG_DESCRIPTOR(1, ITF_NUM_TOTAL, 0, CONFIG_TOTAL_LEN, 0x00, 100),
 
-enum
-{
-  ITF_NUM_HID,
-  ITF_NUM_TOTAL
+	/* Interface number, string index, EP notification address and size,
+	 * EP data address (out, in) and size */
+	TUD_CDC_DESCRIPTOR(ITF_NUM_CDC, 4, EPNUM_CDC_NOTIF, 8,
+		EPNUM_CDC_OUT, EPNUM_CDC_IN, 64),
+
+	/* Interface number, string index, protocol, report descriptor len,
+	 * EP In & Out address, size & polling interval */
+	TUD_HID_INOUT_DESCRIPTOR(ITF_NUM_HID, 5, HID_ITF_PROTOCOL_NONE,
+		sizeof(desc_hid_report), EPNUM_HID, 0x80 | EPNUM_HID, 64, 10)
 };
 
-#define  CONFIG_TOTAL_LEN  (TUD_CONFIG_DESC_LEN + TUD_HID_INOUT_DESC_LEN)
+#if TUD_OPT_HIGH_SPEED
+uint8_t const desc_hs_configuration[] = {
+	/* Config number, interface count, string index,
+	 * total length, attribute, power in mA */
+	TUD_CONFIG_DESCRIPTOR(1, ITF_NUM_TOTAL, 0, CONFIG_TOTAL_LEN, 0x00, 100),
+
+	/* Interface number, string index, EP notification address and size,
+	 * EP data address (out, in) and size */
+	TUD_CDC_DESCRIPTOR(ITF_NUM_CDC, 4, EPNUM_CDC_NOTIF, 8,
+		EPNUM_CDC_OUT, EPNUM_CDC_IN, 512),
+
+	/* Interface number, string index, protocol, report descriptor len,
+	 * EP In & Out address, size & polling interval */
+	TUD_HID_INOUT_DESCRIPTOR(ITF_NUM_HID, 5, HID_ITF_PROTOCOL_NONE,
+		sizeof(desc_hid_report), EPNUM_HID, 0x80 | EPNUM_HID, 512, 10)
+};
 
-#define EPNUM_HID   0x01
+uint8_t desc_other_speed_config[CONFIG_TOTAL_LEN];
 
-uint8_t const desc_configuration[] =
+tusb_desc_device_qualifier_t const desc_device_qualifier =
 {
-  // Config number, interface count, string index, total length, attribute, power in mA
-  TUD_CONFIG_DESCRIPTOR(1, ITF_NUM_TOTAL, 0, CONFIG_TOTAL_LEN, 0x00, 100),
+	.bLength            = sizeof(tusb_desc_device_qualifier_t),
+	.bDescriptorType    = TUSB_DESC_DEVICE_QUALIFIER,
+	.bcdUSB             = USB_BCD,
+
+	.bDeviceClass       = TUSB_CLASS_MISC,
+	.bDeviceSubClass    = MISC_SUBCLASS_COMMON,
+	.bDeviceProtocol    = MISC_PROTOCOL_IAD,
 
-  // Interface number, string index, protocol, report descriptor len, EP In & Out address, size & polling interval
-  TUD_HID_INOUT_DESCRIPTOR(ITF_NUM_HID, 0, HID_ITF_PROTOCOL_NONE, sizeof(desc_hid_report), EPNUM_HID, 0x80 | EPNUM_HID, CFG_TUD_HID_EP_BUFSIZE, 10)
+	.bMaxPacketSize0    = CFG_TUD_ENDPOINT0_SIZE,
+	.bNumConfigurations = 0x01,
+	.bReserved          = 0x00
+};
+#endif
+
+char const *string_desc_arr[] = {
+	[0] = (const char[]) { 0x09, 0x04 }, /* supported language is English */
+	[1] = "TinyUSB",                     /* Manufacturer */
+	[2] = "TinyUSB Device",              /* Product */
+	[3] = "123456",                      /* Serials, should use chip ID */
+	[4] = "SXKBD CDC",
+	[5] = "SXKBD HID"
 };
 
-// Invoked when received GET CONFIGURATION DESCRIPTOR
-// Application return pointer to descriptor
-// Descriptor contents must exist long enough for transfer to complete
-uint8_t const * tud_descriptor_configuration_cb(uint8_t index)
+static uint16_t _desc_str[32];
+
+/* Invoked on GET DEVICE DESCRIPTOR */
+uint8_t const *
+tud_descriptor_device_cb(void)
 {
-  (void) index; // for multiple configurations
-  return desc_configuration;
+	return (uint8_t const *) &desc_device;
 }
 
-//--------------------------------------------------------------------+
-// String Descriptors
-//--------------------------------------------------------------------+
-
-// array of pointer to string descriptors
-char const* string_desc_arr [] =
+/* Invoked on GET HID REPORT DESCRIPTOR */
+uint8_t const *
+tud_hid_descriptor_report_cb(uint8_t itf)
 {
-  (const char[]) { 0x09, 0x04 }, // 0: is supported language is English (0x0409)
-  "TinyUSB",                     // 1: Manufacturer
-  "TinyUSB Device",              // 2: Product
-  "123456",                      // 3: Serials, should use chip ID
-};
+	(void) itf;
+	return desc_hid_report;
+}
 
-static uint16_t _desc_str[32];
+/* Invoked on GET CONFIGURATION DESCRIPTOR */
+uint8_t const *
+tud_descriptor_configuration_cb(uint8_t index)
+{
+	(void) index;
+
+#if TUD_OPT_HIGH_SPEED
+	return (tud_speed_get() == TUSB_SPEED_HIGH) ?
+		desc_hs_configuration : desc_fs_configuration;
+#else
+	return desc_fs_configuration;
+#endif
+}
 
-// Invoked when received GET STRING DESCRIPTOR request
-// Application return pointer to descriptor, whose contents must exist long enough for transfer to complete
-uint16_t const* tud_descriptor_string_cb(uint8_t index, uint16_t langid)
+/* Invoked on GET STRING DESCRIPTOR */
+uint16_t const *
+tud_descriptor_string_cb(uint8_t index, uint16_t langid)
 {
-  (void) langid;
+	const char *str;
+	uint8_t i, chr_count;
+
+	(void) langid;
+
+	if (index == 0) {
+		memcpy(&_desc_str[1], string_desc_arr[0], 2);
+		chr_count = 1;
+	} else {
+		if (index >= ARRLEN(string_desc_arr))
+			return NULL;
 
-  uint8_t chr_count;
+		str = string_desc_arr[index];
 
-  if ( index == 0)
-  {
-    memcpy(&_desc_str[1], string_desc_arr[0], 2);
-    chr_count = 1;
-  }else
-  {
-    // Note: the 0xEE index string is a Microsoft OS 1.0 Descriptors.
-    // https://docs.microsoft.com/en-us/windows-hardware/drivers/usbcon/microsoft-defined-usb-descriptors
+		chr_count = (uint8_t) strlen(str);
+		if (chr_count > 31) chr_count = 31;
 
-    if ( !(index < sizeof(string_desc_arr)/sizeof(string_desc_arr[0])) ) return NULL;
+		/* Convert ASCII string into UTF-16 */
+		for (i = 0; i < chr_count; i++)
+			_desc_str[i+1] = str[i];
+	}
 
-    const char* str = string_desc_arr[index];
+	/* first byte is length (including header), second byte is type */
+	_desc_str[0] = (uint8_t) (2 * chr_count + 2);
+	_desc_str[0] |= (uint16_t) (TUSB_DESC_STRING << 8);
+
+	return _desc_str;
+}
 
-    // Cap at max char
-    chr_count = (uint8_t) strlen(str);
-    if ( chr_count > 31 ) chr_count = 31;
+#if TUD_OPT_HIGH_SPEED
+uint8_t const *
+tud_descriptor_device_qualifier_cb(void)
+{
+	return (uint8_t const*) &desc_device_qualifier;
+}
+
+/* Invoked on GET OTHER SEED CONFIGURATION DESCRIPTOR */
+uint8_t const *
+tud_descriptor_other_speed_configuration_cb(uint8_t index)
+{
+	(void) index;
 
-    // Convert ASCII string into UTF-16
-    for(uint8_t i=0; i<chr_count; i++)
-    {
-      _desc_str[1+i] = str[i];
-    }
-  }
+	memcpy(desc_other_speed_config, (tud_speed_get() == TUSB_SPEED_HIGH)
+		? desc_fs_configuration : desc_hs_configuration,
+		CONFIG_TOTAL_LEN);
 
-  // first byte is length (including header), second byte is string type
-  _desc_str[0] = (uint16_t) ((TUSB_DESC_STRING << 8 ) | (2*chr_count + 2));
+	desc_other_speed_config[1] = TUSB_DESC_OTHER_SPEED_CONFIG;
 
-  return _desc_str;
+	return desc_other_speed_config;
 }
+#endif
diff --git a/src/util.c b/src/util.c
new file mode 100644
index 0000000..5a6cf2d
--- /dev/null
+++ b/src/util.c
@@ -0,0 +1,84 @@
+#include "util.h"
+#include "board.h"
+#include "neopix.h"
+
+#include "tusb.h"
+#include "bsp/board.h"
+#include "tusb_config.h"
+
+#include <stdio.h>
+
+int loglevel = LOG_WARN;
+
+static void
+__attribute__((format(printf, 1, 0)))
+panic_task(const char *fmtstr, va_list ap)
+{
+	va_list cpy;
+	char c;	
+
+	if (!tud_cdc_available())
+		return;
+
+	if (!tud_cdc_read(&c, 1))
+		return;
+
+	va_copy(cpy, ap);
+	vprintf(fmtstr, cpy);
+	printf("\n\r");
+}
+
+static void
+blink_task(struct neopix *pix, uint32_t blink_ms)
+{
+	static uint32_t start_ms = 0;
+	static bool led_state = false;
+
+	if (!start_ms) start_ms = board_millis();
+
+	if (board_millis() - start_ms < blink_ms)
+		return;
+
+	neopix_put(pix, neopix_u32rgb(255 * led_state, 0, 0));
+
+	led_state ^= true;
+	start_ms += blink_ms;
+}
+
+void
+__attribute__ ((format (printf, 2, 3)))
+stdio_log(int level, const char *fmtstr, ...)
+{
+	va_list ap;
+
+	if (!tud_cdc_available())
+		return;
+
+	if (level > loglevel)
+		return;
+
+	va_start(ap, fmtstr);
+	vprintf(fmtstr, ap);
+	va_end(ap);
+	printf("\n\r");
+}
+
+void
+__attribute__((format(printf, 1, 2)))
+blink_panic(const char *fmtstr, ...)
+{
+	va_list ap;
+
+	va_start(ap, fmtstr);
+
+	if (!onboard_led.init)
+		neopix_init(&onboard_led, pio0, 0, ONBOARD_LED_PIN);
+
+	while (1) {
+		tud_task();
+		panic_task(fmtstr, ap);
+		blink_task(&onboard_led, 200);
+	}
+
+	va_end(ap);
+}
diff --git a/src/util.h b/src/util.h
new file mode 100644
index 0000000..e413883
--- /dev/null
+++ b/src/util.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdarg.h>
+
+#define ERR(...) stdio_log(LOG_ERR, __VA_ARGS__)
+#define WARN(...) stdio_log(LOG_WARN, __VA_ARGS__)
+#define INFO(...) stdio_log(LOG_INFO, __VA_ARGS__)
+#define DEBUG(...) stdio_log(LOG_DEBUG, __VA_ARGS__)
+#define PANIC(...) blink_panic(__VA_ARGS__);
+#define ASSERT(cond) blink_panic("Assertion failed: (%s) in %s:%i", \
+	#cond, __FILE__, __LINE__)
+
+enum {
+	LOG_NONE,
+	LOG_ERR,
+	LOG_WARN,
+	LOG_INFO,
+	LOG_DEBUG
+};
+
+void stdio_log(int loglevel, const char *fmtstr, ...);
+
+void blink_panic(const char *fmtstr, ...);
+
+extern int loglevel;