3 files changed, 113 insertions, 10 deletions

diff --git a/crates/rp-pico-2040/Cargo.toml b/crates/rp-pico-2040/Cargo.toml
index 6603bf4..6921f2b 100644
--- a/crates/rp-pico-2040/Cargo.toml
+++ b/crates/rp-pico-2040/Cargo.toml
@@ -8,6 +8,8 @@ embedded-hal = "1.0"
 rp-pico = "0.9.0"
 
 rust-threadx-cortex-m.workspace = true
-rust-threadx-stdio-semihosting.workspace = true
 rust-threadx-sync-single-core.workspace = true
 cortex-m.workspace = true
+
+usb-device.workspace = true
+usbd-serial.workspace = true
diff --git a/crates/rp-pico-2040/src/lib.rs b/crates/rp-pico-2040/src/lib.rs
index 8f877aa..0fb049a 100644
--- a/crates/rp-pico-2040/src/lib.rs
+++ b/crates/rp-pico-2040/src/lib.rs
@@ -11,12 +11,16 @@ extern crate fusible;
 use core::pin::Pin;
 
 use rust_threadx_cortex_m as _;
-use rust_threadx_stdio_semihosting as _;
 use rust_threadx_sync_single_core as _;
 
 use embedded_hal::digital::OutputPin;
 use rp_pico::hal;
 use rp_pico::hal::pac;
+use usb_device::{class_prelude::*, prelude::*};
+use usbd_serial::SerialPort;
+
+extern crate alloc;
+use alloc::vec::Vec;
 
 #[doc(hidden)]
 #[unsafe(no_mangle)]
@@ -45,9 +49,19 @@ pub unsafe extern "C" fn _tx_initialize_low_level() {
     }
 }
 
+use fusible::queue::{Queue, QueueContext, StaticQueueSlots};
+use fusible::semaphore::{Semaphore, SemaphoreContext};
 use fusible::thread::{StaticStack, Thread, ThreadContext};
-static STACK: StaticStack<512> = StaticStack::new();
-static THREAD: ThreadContext = Thread::context();
+
+static BLINK_STACK: StaticStack<512> = StaticStack::new();
+static BLINK_THREAD: ThreadContext = Thread::context();
+
+static USB_STACK: StaticStack<8192> = StaticStack::new();
+static USB_THREAD: ThreadContext = Thread::context();
+static USB_EVENT: SemaphoreContext = Semaphore::context();
+
+static STDOUT_MSGS: StaticQueueSlots<Vec<u8>, 4> = StaticQueueSlots::new();
+static STDOUT_QUEUE: QueueContext<Vec<u8>> = Queue::context();
 
 #[unsafe(no_mangle)]
 #[doc(hidden)]
@@ -56,7 +70,7 @@ pub extern "C" fn __rust_threadx_app_define() {
 
     let mut watchdog = hal::Watchdog::new(pac.WATCHDOG);
 
-    hal::clocks::init_clocks_and_plls(
+    let clocks = hal::clocks::init_clocks_and_plls(
         rp_pico::XOSC_CRYSTAL_FREQ,
         pac.XOSC,
         pac.CLOCKS,
@@ -80,8 +94,8 @@ pub extern "C" fn __rust_threadx_app_define() {
     let mut led_pin = pins.led.into_push_pull_output();
 
     Thread::create(
-        Pin::static_ref(&THREAD),
-        STACK.take().unwrap(),
+        Pin::static_ref(&BLINK_THREAD),
+        BLINK_STACK.take().unwrap(),
         &Default::default(),
         move || loop {
             led_pin.set_high().unwrap();
@@ -91,4 +105,90 @@ pub extern "C" fn __rust_threadx_app_define() {
         },
     )
     .unwrap();
+
+    let usb_event = Semaphore::create(Pin::static_ref(&USB_EVENT), &Default::default()).unwrap();
+    let stdout_queue = Queue::create(
+        Pin::static_ref(&STDOUT_QUEUE),
+        STDOUT_MSGS.take().unwrap(),
+        &Default::default(),
+    )
+    .unwrap();
+
+    Thread::create(
+        Pin::static_ref(&USB_THREAD),
+        USB_STACK.take().unwrap(),
+        &Default::default(),
+        move || {
+            let usb_bus = UsbBusAllocator::new(hal::usb::UsbBus::new(
+                pac.USBCTRL_REGS,
+                pac.USBCTRL_DPRAM,
+                clocks.usb_clock,
+                true,
+                &mut pac.RESETS,
+            ));
+
+            let mut serial = SerialPort::new(&usb_bus);
+            let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x16c0, 0x27dd))
+                .strings(&[StringDescriptors::default()
+                    .manufacturer("Fake company")
+                    .product("Serial port")
+                    .serial_number("TEST")])
+                .unwrap()
+                .device_class(2)
+                .build();
+
+            loop {
+                unsafe { cortex_m::peripheral::NVIC::unmask(hal::pac::Interrupt::USBCTRL_IRQ) };
+                usb_event.get().unwrap();
+                usb_dev.poll(&mut [&mut serial]);
+
+                serial.read(&mut []).ok();
+
+                if let Some(msg) = stdout_queue.try_receive() {
+                    let _ = serial.write(&msg);
+                }
+            }
+        },
+    )
+    .unwrap();
+}
+
+use hal::pac::interrupt;
+
+#[interrupt]
+fn USBCTRL_IRQ() {
+    cortex_m::peripheral::NVIC::mask(hal::pac::interrupt::USBCTRL_IRQ);
+
+    // Safety: If the interrupt is unmasked, then we created
+    // the semaphore.
+    unsafe { Pin::static_ref(&USB_EVENT).assume_created().put() };
+}
+
+fn stdout_write(buf: &[u8]) -> usize {
+    let Some(queue) = Pin::static_ref(&STDOUT_QUEUE).try_created() else {
+        return 0;
+    };
+
+    let msg = Vec::from(buf);
+    queue.send(msg).unwrap();
+    // Wake up the USB thread so it can see our message.
+    if let Some(usb_event) = Pin::static_ref(&USB_EVENT).try_created() {
+        usb_event.put();
+    }
+    buf.len()
+}
+
+#[unsafe(no_mangle)]
+fn __rust_threadx_stdout_write(buf: &[u8]) -> usize {
+    stdout_write(buf)
+}
+
+#[unsafe(no_mangle)]
+fn __rust_threadx_stderr_write(buf: &[u8]) -> usize {
+    stdout_write(buf)
+}
+
+#[unsafe(no_mangle)]
+fn __rust_threadx_stdin_read(_: &mut [u8]) -> usize {
+    0 // Haven't tried it.
 }
diff --git a/crates/rp-pico-2040/src/main.rs b/crates/rp-pico-2040/src/main.rs
index b155870..389257a 100644
--- a/crates/rp-pico-2040/src/main.rs
+++ b/crates/rp-pico-2040/src/main.rs
@@ -3,9 +3,10 @@ use rust_threadx_rp_pico_2040 as _;
 use std::{thread, time::Duration};
 
 fn main() {
-    // Nothing interesting yet. Nevertheless, the LED should
-    // be blinking on a background thread.
+    let mut count = 0_usize;
     loop {
-        thread::sleep(Duration::from_millis(100));
+        println!("Hello world! The count is {count}\r");
+        thread::sleep(Duration::from_millis(1_000));
+        count = count.wrapping_add(1);
     }
 }