main: handle RP2040's broken USB error handling

RP2040-E15 can also be triggered if a Debug Probe is connected to a
board with a floating ground. Typically this causes port ESD protection
to temporarily activate, meaning the Dp/Dm state gets corrupted. If this
happens in the middle of a handshake packet, the SIE can lock up.

The only way to detect this case is if SOF_RD stops advancing without a
corresponding suspend interrupt - so add a watchdog thread that forces a
disconnect if the hardware stops reporting frame counts.

This is disruptive, but immediate notification that the probe broke is
preferable to silently failing until the next character is sent by the host.

Signed-off-by: Jonathan Bell <jonathan@raspberrypi.com>

src/main.c

diff --git a/src/main.c b/src/main.c
index 90db00b..115c9ad 100644
--- a/src/main.c
+++ b/src/main.c
@@ -44,6 +44,7 @@
 #include "get_serial.h"
 #include "tusb_edpt_handler.h"
 #include "DAP.h"
+#include "hardware/structs/usb.h"
 
 // UART0 for debugprobe debug
 // UART1 for debugprobe to target device
@@ -57,7 +58,36 @@
 #define TUD_TASK_PRIO  (tskIDLE_PRIORITY + 2)
 #define DAP_TASK_PRIO  (tskIDLE_PRIORITY + 1)
 
-TaskHandle_t dap_taskhandle, tud_taskhandle;
+TaskHandle_t dap_taskhandle, tud_taskhandle, mon_taskhandle;
+
+void dev_mon(void *ptr)
+{
+    uint32_t sof[3];
+    int i = 0;
+    TickType_t wake;
+    wake = xTaskGetTickCount();
+    do {
+        /* ~5 SOF events per tick */
+        xTaskDelayUntil(&wake, 100);
+        if (tud_connected() && !tud_suspended()) {
+            sof[i++] = usb_hw->sof_rd & USB_SOF_RD_BITS;
+            i = i % 3;
+        } else {
+            for (i = 0; i < 3; i++)
+                sof[i] = 0;
+        }
+        if ((sof[0] | sof[1] | sof[2]) != 0) {
+            if ((sof[0] == sof[1]) && (sof[1] == sof[2])) {
+                probe_info("Watchdog timeout! Resetting USBD\n");
+                /* uh oh, signal disconnect (implicitly resets the controller) */
+                tud_deinit(0);
+                /* Make sure the port got the message */
+                xTaskDelayUntil(&wake, 1);
+                tud_init(0);
+            }
+        }
+    } while (1);
+}
 
 void usb_thread(void *ptr)
 {
@@ -104,11 +134,10 @@
     probe_info("Welcome to debugprobe!\n");
 
     if (THREADED) {
-        /* UART needs to preempt USB as if we don't, characters get lost */
-        xTaskCreate(cdc_thread, "UART", configMINIMAL_STACK_SIZE, NULL, UART_TASK_PRIO, &uart_taskhandle);
         xTaskCreate(usb_thread, "TUD", configMINIMAL_STACK_SIZE, NULL, TUD_TASK_PRIO, &tud_taskhandle);
-        /* Lowest priority thread is debug - need to shuffle buffers before we can toggle swd... */
-        xTaskCreate(dap_thread, "DAP", configMINIMAL_STACK_SIZE, NULL, DAP_TASK_PRIO, &dap_taskhandle);
+#if PICO_RP2040
+        xTaskCreate(dev_mon, "WDOG", configMINIMAL_STACK_SIZE, NULL, TUD_TASK_PRIO, &mon_taskhandle);
+#endif
         vTaskStartScheduler();
     }