From e750920d4b5bdf7decc090848db8b464bace86d1 Mon Sep 17 00:00:00 2001 From: Michael van Niekerk Date: Tue, 6 Dec 2022 10:06:42 +0200 Subject: [PATCH 1/3] USB to UART passthrough --- boards/rp-pico/Cargo.toml | 4 + boards/rp-pico/examples/pico_usb_to_uart.rs | 324 ++++++++++++++++++++ 2 files changed, 328 insertions(+) create mode 100644 boards/rp-pico/examples/pico_usb_to_uart.rs diff --git a/boards/rp-pico/Cargo.toml b/boards/rp-pico/Cargo.toml index 719e1a0b6..372e0ac19 100644 --- a/boards/rp-pico/Cargo.toml +++ b/boards/rp-pico/Cargo.toml @@ -70,3 +70,7 @@ rom-v2-intrinsics = ["rp2040-hal/rom-v2-intrinsics"] [[example]] name = "pico_rtic_monotonic" required-features = ["rp2040-hal/rtic-monotonic"] + +[[example]] +name = "pico_usb_to_uart" +required-features = ["rp2040-hal/rtic-monotonic"] diff --git a/boards/rp-pico/examples/pico_usb_to_uart.rs b/boards/rp-pico/examples/pico_usb_to_uart.rs new file mode 100644 index 000000000..137b92608 --- /dev/null +++ b/boards/rp-pico/examples/pico_usb_to_uart.rs @@ -0,0 +1,324 @@ +//! # Pico USB to UART passthrough +//! +//! Creates a USB Serial device on a Pico board, with the USB driver running in +//! the USB interrupt. +//! +//! This will create a USB Serial device passing the data through to a UART. UART incoming +//! data in turn is then sent back over the USB. +//! +//! See the `Cargo.toml` file for Copyright and license details. +#![no_std] +#![no_main] + +use panic_halt as _; +#[rtic::app(device = rp_pico::hal::pac, peripherals = true, dispatchers = [I2C0_IRQ])] +mod app { + + // GPIO traits + use fugit::RateExtU32; + use fugit::ExtU64; + + // Ensure we halt the program on panic (if we don't mention this crate it won't + // be linked) + use panic_halt as _; + + // Pull in any important traits + use rp_pico::hal::prelude::*; + + // A shorter alias for the Peripheral Access Crate, which provides low-level + // register access + use rp_pico::hal::pac; + + // USB Device support + use usb_device::{class_prelude::*, prelude::*}; + + // USB Communications Class Device support + use usbd_serial::SerialPort; + + /// Import the GPIO pins we use + use hal::gpio::pin::bank0::{Gpio0, Gpio1}; + + // UART related types + use hal::uart::{DataBits, StopBits, UartConfig}; + + // These are the traits we need from Embedded HAL to treat our hardware + // objects as generic embedded devices. + use embedded_hal::{ + digital::v2::OutputPin, + serial::{Read, Write}, + }; + + /// Alias the type for our UART pins to make things clearer. + type UartPins = ( + hal::gpio::Pin>, + hal::gpio::Pin>, + ); + + /// Alias the type for our UART to make things clearer. + type Uart = hal::uart::UartPeripheral; + + use heapless::spsc::{Consumer, Producer, Queue}; + use rp_pico::{ + hal::{ + self, + clocks::init_clocks_and_plls, + watchdog::Watchdog, + Sio, + timer::{monotonic::Monotonic, Alarm0}, + }, + XOSC_CRYSTAL_FREQ, + }; + const BUF_SIZE: usize = 64; + const DOUBLE_BUF_SIZE: usize = BUF_SIZE * 2; + + /// Used to leak a &'static reference on the USB allocator + static mut USB_BUS: Option> = None; + + /// Used for &'static references for the queues + static mut USB_QUEUE: Queue = Queue::new(); + static mut UART_QUEUE: Queue = Queue::new(); + + #[shared] + struct Shared { + led: hal::gpio::Pin, + uart: Uart, + serial: SerialPort<'static, hal::usb::UsbBus>, + } + + #[local] + struct Local { + usb_device: UsbDevice<'static, hal::usb::UsbBus>, + usb_producer: Producer<'static, u8, DOUBLE_BUF_SIZE>, + usb_consumer: Consumer<'static, u8, DOUBLE_BUF_SIZE>, + uart_producer: Producer<'static, u8, DOUBLE_BUF_SIZE>, + uart_consumer: Consumer<'static, u8, DOUBLE_BUF_SIZE>, + } + + #[monotonic(binds = TIMER_IRQ_0, default = true)] + type MyMono = Monotonic; + + #[init] + fn init(c: init::Context) -> (Shared, Local, init::Monotonics) { + // Soft-reset does not release the hardware spinlocks + // Release them now to avoid a deadlock after debug or watchdog reset + unsafe { + hal::sio::spinlock_reset(); + } + let mut resets = c.device.RESETS; + let mut watchdog = Watchdog::new(c.device.WATCHDOG); + let clocks = init_clocks_and_plls( + XOSC_CRYSTAL_FREQ, + c.device.XOSC, + c.device.CLOCKS, + c.device.PLL_SYS, + c.device.PLL_USB, + &mut resets, + &mut watchdog, + ) + .ok() + .unwrap(); + + let sio = Sio::new(c.device.SIO); + let pins = rp_pico::Pins::new( + c.device.IO_BANK0, + c.device.PADS_BANK0, + sio.gpio_bank0, + &mut resets, + ); + let mut led = pins.led.into_push_pull_output(); + led.set_low().unwrap(); + + let usb_bus = UsbBusAllocator::new(hal::usb::UsbBus::new( + c.device.USBCTRL_REGS, + c.device.USBCTRL_DPRAM, + clocks.usb_clock, + true, + &mut resets, + )); + + unsafe { + // Note (safety): This is safe as interrupts haven't been started yet + USB_BUS = Some(usb_bus); + } + + // Grab a reference to the USB Bus allocator. We are promising to the + // compiler not to take mutable access to this global variable whilst this + // reference exists! + let bus_ref = unsafe { USB_BUS.as_ref().unwrap() }; + + let serial = SerialPort::new(bus_ref); + + let usb_bus = unsafe { USB_BUS.as_mut().unwrap() }; + + // Create a USB device with a fake VID and PID + let usb_device = UsbDeviceBuilder::new(usb_bus, UsbVidPid(0x16c0, 0x27dd)) + .manufacturer("Fake company") + .product("Serial port") + .serial_number("MJVN") + .device_class(2) // from: https://www.usb.org/defined-class-codes + .build(); + + let uart_pins = ( + // UART TX (characters sent from RP2040) on pin 1 (GPIO0) + pins.gpio0.into_mode::(), + // UART RX (characters received by RP2040) on pin 2 (GPIO1) + pins.gpio1.into_mode::(), + ); + + // Make a UART on the given pins + let mut uart = hal::uart::UartPeripheral::new(c.device.UART0, uart_pins, &mut resets) + .enable( + UartConfig::new(115200.Hz(), DataBits::Eight, None, StopBits::One), + clocks.peripheral_clock.freq(), + ) + .unwrap(); + + // Tell the UART to raise its interrupt line on the NVIC when the RX FIFO + // has data in it. + uart.enable_rx_interrupt(); + + let (usb_producer, usb_consumer) = unsafe { USB_QUEUE.split() }; + let (uart_producer, uart_consumer) = unsafe { UART_QUEUE.split() }; + + let mut timer = hal::Timer::new(c.device.TIMER, &mut resets); + let alarm = timer.alarm_0().unwrap(); + blink_led::spawn_after(500.millis()).unwrap(); + + ( + Shared { + led, + serial, + uart, + }, + Local { + usb_producer, + usb_consumer, + uart_producer, + uart_consumer, + usb_device, + }, + init::Monotonics(Monotonic::new(timer, alarm)), + ) + } + + #[task( + binds = UART0_IRQ, + priority = 1, + shared = [uart], + local = [usb_producer] + )] + fn uart_read(mut c: uart_read::Context) { + let capacity = c.local.usb_producer.capacity(); + let len = c.local.usb_producer.len(); + // Check if we can take at least the buffer + let ready = capacity - len > BUF_SIZE; + if !ready { + // Queue is full. Retry in 1ms + pac::NVIC::pend(hal::pac::Interrupt::UART0_IRQ); + } + + while let Ok(byte) = c.shared.uart.lock(|uart| uart.read()) { + c.local.usb_producer.enqueue(byte).unwrap(); + + let len = c.local.usb_producer.len(); + if len > BUF_SIZE { + usb_write::spawn_after(0.millis()).unwrap(); + } else if len == DOUBLE_BUF_SIZE { + // Queue is full. Retry again + pac::NVIC::pend(hal::pac::Interrupt::UART0_IRQ); + } + } + usb_write::spawn_after(0.millis()).unwrap(); + } + + #[task( + binds = USBCTRL_IRQ, + priority = 1, + shared = [serial], + local = [usb_device, uart_producer] + )] + fn usb_read(mut c: usb_read::Context) { + let mut buf = [0u8; BUF_SIZE]; + loop { + let capacity = c.local.uart_producer.capacity(); + let len = c.local.uart_producer.len(); + // Check if we can take at least the buffer + let ready = capacity - len > BUF_SIZE; + if !ready { + // Queue is full. Retry again + pac::NVIC::pend(hal::pac::Interrupt::USBCTRL_IRQ); + break; + } + let usb_device = &mut c.local.usb_device; + let uart_producer = &mut c.local.uart_producer; + let count = c.shared.serial.lock(|serial| { + if usb_device.poll(&mut [serial]) { + match serial.read(&mut buf) { + Ok(count) => { + buf.iter().take(count).for_each(|b| + uart_producer.enqueue(*b).ok().unwrap() + ); + uart_write::spawn_after(0.millis()).unwrap(); + count + } + _ => 0 + } + } else { + 0 + } + }); + + if count == 0 { + break; + } + } + } + + #[task( + priority = 1, + shared = [uart], + local = [uart_consumer] + )] + fn uart_write(mut c: uart_write::Context) { + while let Some(b) = c.local.uart_consumer.dequeue() { + let _ = c.shared.uart.lock(|uart| uart.write(b)); + } + } + + #[task( + priority = 1, + shared = [serial], + local = [usb_consumer] + )] + fn usb_write(mut c: usb_write::Context) { + let mut buf = [0u8; BUF_SIZE]; + let mut ptr = 0usize; + while let Some(b) = c.local.usb_consumer.dequeue() { + buf[ptr] = b; + ptr += 1; + if ptr == BUF_SIZE { + let _ = c.shared.serial.lock(|serial| serial.write(&buf)); + ptr = 0; + } + } + if ptr > 0 { + let buf_ptr = &buf[0..ptr]; + let _ = c.shared.serial.lock(|serial| serial.write(buf_ptr)); + } + } + + #[task( + shared = [led], + local = [tog: bool = true], + )] + fn blink_led(mut c: blink_led::Context) { + if *c.local.tog { + c.shared.led.lock(|l| l.set_high().unwrap()); + } else { + c.shared.led.lock(|l| l.set_low().unwrap()); + } + *c.local.tog = !*c.local.tog; + + blink_led::spawn_after(500.millis()).unwrap(); + } +} From 2a89c62185bf8f547454c3d6c20fa6601e6f3b10 Mon Sep 17 00:00:00 2001 From: Michael van Niekerk Date: Tue, 6 Dec 2022 18:34:29 +0200 Subject: [PATCH 2/3] Does not lock up --- boards/rp-pico/Cargo.toml | 4 - boards/rp-pico/examples/pico_usb_to_uart.rs | 61 +++- .../examples/pico_usb_uart_passthrough.rs | 330 ++++++++++++++++++ 3 files changed, 372 insertions(+), 23 deletions(-) create mode 100644 boards/rp-pico/examples/pico_usb_uart_passthrough.rs diff --git a/boards/rp-pico/Cargo.toml b/boards/rp-pico/Cargo.toml index 372e0ac19..719e1a0b6 100644 --- a/boards/rp-pico/Cargo.toml +++ b/boards/rp-pico/Cargo.toml @@ -70,7 +70,3 @@ rom-v2-intrinsics = ["rp2040-hal/rom-v2-intrinsics"] [[example]] name = "pico_rtic_monotonic" required-features = ["rp2040-hal/rtic-monotonic"] - -[[example]] -name = "pico_usb_to_uart" -required-features = ["rp2040-hal/rtic-monotonic"] diff --git a/boards/rp-pico/examples/pico_usb_to_uart.rs b/boards/rp-pico/examples/pico_usb_to_uart.rs index 137b92608..67ebabca7 100644 --- a/boards/rp-pico/examples/pico_usb_to_uart.rs +++ b/boards/rp-pico/examples/pico_usb_to_uart.rs @@ -11,11 +11,11 @@ #![no_main] use panic_halt as _; -#[rtic::app(device = rp_pico::hal::pac, peripherals = true, dispatchers = [I2C0_IRQ])] +#[rtic::app(device = rp_pico::hal::pac, peripherals = true, dispatchers = [I2C0_IRQ, I2C1_IRQ])] mod app { // GPIO traits - use fugit::RateExtU32; + use fugit::{MicrosDurationU32, RateExtU32}; use fugit::ExtU64; // Ensure we halt the program on panic (if we don't mention this crate it won't @@ -58,13 +58,14 @@ mod app { type Uart = hal::uart::UartPeripheral; use heapless::spsc::{Consumer, Producer, Queue}; + use rp_pico::{ hal::{ self, clocks::init_clocks_and_plls, watchdog::Watchdog, Sio, - timer::{monotonic::Monotonic, Alarm0}, + timer::Alarm }, XOSC_CRYSTAL_FREQ, }; @@ -78,11 +79,15 @@ mod app { static mut USB_QUEUE: Queue = Queue::new(); static mut UART_QUEUE: Queue = Queue::new(); + const SCAN_TIME_US: MicrosDurationU32 = MicrosDurationU32::secs(1); + #[shared] struct Shared { led: hal::gpio::Pin, uart: Uart, serial: SerialPort<'static, hal::usb::UsbBus>, + timer: hal::Timer, + alarm: hal::timer::Alarm0, } #[local] @@ -94,11 +99,8 @@ mod app { uart_consumer: Consumer<'static, u8, DOUBLE_BUF_SIZE>, } - #[monotonic(binds = TIMER_IRQ_0, default = true)] - type MyMono = Monotonic; - #[init] - fn init(c: init::Context) -> (Shared, Local, init::Monotonics) { + fn init(mut c: init::Context) -> (Shared, Local, init::Monotonics) { // Soft-reset does not release the hardware spinlocks // Release them now to avoid a deadlock after debug or watchdog reset unsafe { @@ -181,14 +183,19 @@ mod app { let (uart_producer, uart_consumer) = unsafe { UART_QUEUE.split() }; let mut timer = hal::Timer::new(c.device.TIMER, &mut resets); - let alarm = timer.alarm_0().unwrap(); - blink_led::spawn_after(500.millis()).unwrap(); + let mut alarm = timer.alarm_0().unwrap(); + let _ = alarm.schedule(SCAN_TIME_US); + alarm.enable_interrupt(); + + c.core.SCB.set_sleepdeep(); ( Shared { led, serial, uart, + timer, + alarm }, Local { usb_producer, @@ -197,10 +204,20 @@ mod app { uart_consumer, usb_device, }, - init::Monotonics(Monotonic::new(timer, alarm)), + init::Monotonics() ) } + #[idle] + fn idle(cx: idle::Context) -> ! { + loop { + // Now Wait For Interrupt is used instead of a busy-wait loop + // to allow MCU to sleep between interrupts + // https://developer.arm.com/documentation/ddi0406/c/Application-Level-Architecture/Instruction-Details/Alphabetical-list-of-instructions/WFI + rtic::export::wfi() + } + } + #[task( binds = UART0_IRQ, priority = 1, @@ -222,18 +239,18 @@ mod app { let len = c.local.usb_producer.len(); if len > BUF_SIZE { - usb_write::spawn_after(0.millis()).unwrap(); + usb_write::spawn().unwrap(); } else if len == DOUBLE_BUF_SIZE { // Queue is full. Retry again pac::NVIC::pend(hal::pac::Interrupt::UART0_IRQ); } } - usb_write::spawn_after(0.millis()).unwrap(); + usb_write::spawn().unwrap(); } #[task( binds = USBCTRL_IRQ, - priority = 1, + priority = 2, shared = [serial], local = [usb_device, uart_producer] )] @@ -258,7 +275,7 @@ mod app { buf.iter().take(count).for_each(|b| uart_producer.enqueue(*b).ok().unwrap() ); - uart_write::spawn_after(0.millis()).unwrap(); + uart_write::spawn().unwrap(); count } _ => 0 @@ -275,7 +292,7 @@ mod app { } #[task( - priority = 1, + priority = 3, shared = [uart], local = [uart_consumer] )] @@ -286,7 +303,7 @@ mod app { } #[task( - priority = 1, + priority = 4, shared = [serial], local = [usb_consumer] )] @@ -308,10 +325,12 @@ mod app { } #[task( - shared = [led], + binds = TIMER_IRQ_0, + priority = 1, + shared = [timer, alarm, led], local = [tog: bool = true], )] - fn blink_led(mut c: blink_led::Context) { + fn timer_irq(mut c: timer_irq::Context) { if *c.local.tog { c.shared.led.lock(|l| l.set_high().unwrap()); } else { @@ -319,6 +338,10 @@ mod app { } *c.local.tog = !*c.local.tog; - blink_led::spawn_after(500.millis()).unwrap(); + let mut alarm = c.shared.alarm; + (alarm).lock(|a| { + a.clear_interrupt(); + let _ = a.schedule(SCAN_TIME_US); + }); } } diff --git a/boards/rp-pico/examples/pico_usb_uart_passthrough.rs b/boards/rp-pico/examples/pico_usb_uart_passthrough.rs new file mode 100644 index 000000000..57502735a --- /dev/null +++ b/boards/rp-pico/examples/pico_usb_uart_passthrough.rs @@ -0,0 +1,330 @@ +//! # Pico USB Serial (with Interrupts) Example +//! +//! Creates a USB Serial device on a Pico board, with the USB driver running in +//! the USB interrupt. +//! +//! This will create a USB Serial device echoing anything it receives. Incoming +//! ASCII characters are converted to upercase, so you can tell it is working +//! and not just local-echo! +//! +//! See the `Cargo.toml` file for Copyright and license details. + +#![no_std] +#![no_main] + +// The macro for our start-up function +use rp_pico::entry; + +// The macro for marking our interrupt functions +use rp_pico::hal::pac::interrupt; + +// GPIO traits +use fugit::RateExtU32; + +// Ensure we halt the program on panic (if we don't mention this crate it won't +// be linked) +use panic_halt as _; + +// Pull in any important traits +use rp_pico::hal::prelude::*; + +// A shorter alias for the Peripheral Access Crate, which provides low-level +// register access +use rp_pico::hal::pac; + +// A shorter alias for the Hardware Abstraction Layer, which provides +// higher-level drivers. +use rp_pico::hal; + +// USB Device support +use usb_device::{class_prelude::*, prelude::*}; + +// USB Communications Class Device support +use usbd_serial::SerialPort; + + +/// Import the GPIO pins we use +use hal::gpio::pin::bank0::{Gpio0, Gpio1}; + +// UART related types +use hal::uart::{DataBits, StopBits, UartConfig}; + +// These are the traits we need from Embedded HAL to treat our hardware +// objects as generic embedded devices. +use embedded_hal::{ + digital::v2::OutputPin, + serial::{Read, Write}, +}; + +/// Alias the type for our UART pins to make things clearer. +type UartPins = ( + hal::gpio::Pin>, + hal::gpio::Pin>, +); + +/// Alias the type for our UART to make things clearer. +type Uart = hal::uart::UartPeripheral; + +/// The USB Device Driver (shared with the interrupt). +static mut USB_DEVICE: Option> = None; + +/// The USB Bus Driver (shared with the interrupt). +static mut USB_BUS: Option> = None; + +/// The USB Serial Device Driver (shared with the interrupt). +static mut USB_SERIAL: Option> = None; + +/// This how we transfer the UART into the Interrupt Handler +static mut GLOBAL_UART: Option = None; + +static mut USB_WRITE_BUF: [u8; 1024] = [0u8; 1024]; + +/// Entry point to our bare-metal application. +/// +/// The `#[entry]` macro ensures the Cortex-M start-up code calls this function +/// as soon as all global variables are initialised. +/// +/// The function configures the RP2040 peripherals, then blinks the LED in an +/// infinite loop. +#[entry] +fn main() -> ! { + // Grab our singleton objects + let mut pac = pac::Peripherals::take().unwrap(); + let core = pac::CorePeripherals::take().unwrap(); + + // Set up the watchdog driver - needed by the clock setup code + let mut watchdog = hal::Watchdog::new(pac.WATCHDOG); + + // Configure the clocks + // + // The default is to generate a 125 MHz system clock + let clocks = hal::clocks::init_clocks_and_plls( + rp_pico::XOSC_CRYSTAL_FREQ, + pac.XOSC, + pac.CLOCKS, + pac.PLL_SYS, + pac.PLL_USB, + &mut pac.RESETS, + &mut watchdog, + ) + .ok() + .unwrap(); + + // Set up the USB driver + let usb_bus = UsbBusAllocator::new(hal::usb::UsbBus::new( + pac.USBCTRL_REGS, + pac.USBCTRL_DPRAM, + clocks.usb_clock, + true, + &mut pac.RESETS, + )); + unsafe { + // Note (safety): This is safe as interrupts haven't been started yet + USB_BUS = Some(usb_bus); + } + + // Grab a reference to the USB Bus allocator. We are promising to the + // compiler not to take mutable access to this global variable whilst this + // reference exists! + let bus_ref = unsafe { USB_BUS.as_ref().unwrap() }; + + // Set up the USB Communications Class Device driver + let serial = SerialPort::new(bus_ref); + unsafe { + USB_SERIAL = Some(serial); + } + + // Create a USB device with a fake VID and PID + let usb_dev = UsbDeviceBuilder::new(bus_ref, UsbVidPid(0x16c0, 0x27dd)) + .manufacturer("Fake company") + .product("Serial port") + .serial_number("MJVN") + .device_class(2) // from: https://www.usb.org/defined-class-codes + .build(); + unsafe { + // Note (safety): This is safe as interrupts haven't been started yet + USB_DEVICE = Some(usb_dev); + } + + // Enable the USB interrupt + unsafe { + pac::NVIC::unmask(hal::pac::Interrupt::USBCTRL_IRQ); + }; + + // No more USB code after this point in main! We can do anything we want in + // here since USB is handled in the interrupt - let's blink an LED! + + // The delay object lets us wait for specified amounts of time (in + // milliseconds) + let mut delay = cortex_m::delay::Delay::new(core.SYST, clocks.system_clock.freq().to_Hz()); + + // The single-cycle I/O block controls our GPIO pins + let sio = hal::Sio::new(pac.SIO); + + // Set the pins up according to their function on this particular board + let pins = rp_pico::Pins::new( + pac.IO_BANK0, + pac.PADS_BANK0, + sio.gpio_bank0, + &mut pac.RESETS, + ); + + + let uart_pins = ( + // UART TX (characters sent from RP2040) on pin 1 (GPIO0) + pins.gpio0.into_mode::(), + // UART RX (characters received by RP2040) on pin 2 (GPIO1) + pins.gpio1.into_mode::(), + ); + + // Make a UART on the given pins + let mut uart = hal::uart::UartPeripheral::new(pac.UART0, uart_pins, &mut pac.RESETS) + .enable( + UartConfig::new(115200.Hz(), DataBits::Eight, None, StopBits::One), + clocks.peripheral_clock.freq(), + ) + .unwrap(); + + // Tell the UART to raise its interrupt line on the NVIC when the RX FIFO + // has data in it. + uart.enable_rx_interrupt(); + + unsafe { + // Note (safety): This is safe as interrupts haven't been started yet + GLOBAL_UART = Some(uart); + } + + unsafe { + // Enable the UART interrupt in the *Nested Vectored Interrupt + // Controller*, which is part of the Cortex-M0+ core. + pac::NVIC::unmask(hal::pac::Interrupt::UART0_IRQ); + } + + + // Set the LED to be an output + let mut led_pin = pins.led.into_push_pull_output(); + + // Blink the LED at 1 Hz + loop { + led_pin.set_high().unwrap(); + delay.delay_ms(500); + led_pin.set_low().unwrap(); + delay.delay_ms(500); + } + // Ok(()) +} + +/// This function is called whenever the USB Hardware generates an Interrupt +/// Request. +/// +/// We do all our USB work under interrupt, so the main thread can continue on +/// knowing nothing about USB. +#[allow(non_snake_case)] +#[interrupt] +unsafe fn USBCTRL_IRQ() { + use core::sync::atomic::{AtomicBool, Ordering}; + + // Disable the USB interrupt + // unsafe { + // pac::NVIC::mask(hal::pac::Interrupt::USBCTRL_IRQ); + // }; + + /// Note whether we've already printed the "hello" message. + static SAID_HELLO: AtomicBool = AtomicBool::new(false); + + // Grab the global objects. This is OK as we only access them under interrupt. + let usb_dev = USB_DEVICE.as_mut().unwrap(); + let serial = USB_SERIAL.as_mut().unwrap(); + let uart = GLOBAL_UART.as_mut().unwrap(); + // let mut buf = USB_WRITE_BUF.as_mut().unwrap(); + + // Say hello exactly once on start-up + // if !SAID_HELLO.load(Ordering::Relaxed) { + // SAID_HELLO.store(true, Ordering::Relaxed); + // let _ = serial.write(b"Hello, World!\r\n"); + // } + + // Poll the USB driver with all of our supported USB Classes + if usb_dev.poll(&mut [serial]) { + + loop { + match serial.read(&mut USB_WRITE_BUF) { + Err(_e) => { + // Do nothing + break; + } + Ok(0) => { + // Do nothing + break; + } + Ok(count) => { + // Convert to upper case + USB_WRITE_BUF.iter().take(count).for_each(|b| { + uart.write(*b); + }); + } + } + } + } + + // Enable the USB interrupt + // unsafe { + // pac::NVIC::unmask(hal::pac::Interrupt::USBCTRL_IRQ); + // }; +} + +#[interrupt] +unsafe fn UART0_IRQ() { + // Disable the USB interrupt + // unsafe { + // pac::NVIC::mask(hal::pac::Interrupt::UART0_IRQ); + // }; + let uart = GLOBAL_UART.as_mut().unwrap(); + let serial = USB_SERIAL.as_mut().unwrap(); + // Echo the input back to the output until the FIFO is empty. Reading + // from the UART should also clear the UART interrupt flag. + let mut buf = [0u8; 32]; + let mut ptr = 0; + while let Ok(byte) = uart.read() { + buf[ptr] = byte; + if ptr + 1 >= buf.len() { + let mut write_ptr = 0; + loop { + let mut data = &buf[write_ptr..ptr]; + let len = serial.write(data); + if let Ok(len) = len { + write_ptr += len; + if write_ptr < ptr { + continue; + } + } + break; + } + ptr = 0; + } else { + ptr += 1; + } + } + if ptr > 0 { + let mut write_ptr = 0; + loop { + let mut data = &buf[write_ptr..ptr]; + let len = serial.write(data); + if let Ok(len) = len { + write_ptr += len; + if write_ptr < ptr { + continue; + } + } + break; + } + } + // Enable the UART interrupt + // unsafe { + // pac::NVIC::unmask(hal::pac::Interrupt::UART0_IRQ); + // }; + // Set an event to ensure the main thread always wakes up, even if it's in + // the process of going to sleep. + cortex_m::asm::sev(); +} +// End of file From 1b96d7594eb969f4c5ad9ab503a5bf820683098b Mon Sep 17 00:00:00 2001 From: Michael van Niekerk Date: Tue, 6 Dec 2022 19:41:32 +0200 Subject: [PATCH 3/3] Doesn't block anymore, format it --- boards/rp-pico/examples/pico_usb_to_uart.rs | 26 +- .../examples/pico_usb_uart_passthrough.rs | 330 ------------------ 2 files changed, 10 insertions(+), 346 deletions(-) delete mode 100644 boards/rp-pico/examples/pico_usb_uart_passthrough.rs diff --git a/boards/rp-pico/examples/pico_usb_to_uart.rs b/boards/rp-pico/examples/pico_usb_to_uart.rs index 67ebabca7..7f9e12de2 100644 --- a/boards/rp-pico/examples/pico_usb_to_uart.rs +++ b/boards/rp-pico/examples/pico_usb_to_uart.rs @@ -15,8 +15,8 @@ use panic_halt as _; mod app { // GPIO traits - use fugit::{MicrosDurationU32, RateExtU32}; use fugit::ExtU64; + use fugit::{MicrosDurationU32, RateExtU32}; // Ensure we halt the program on panic (if we don't mention this crate it won't // be linked) @@ -60,13 +60,7 @@ mod app { use heapless::spsc::{Consumer, Producer, Queue}; use rp_pico::{ - hal::{ - self, - clocks::init_clocks_and_plls, - watchdog::Watchdog, - Sio, - timer::Alarm - }, + hal::{self, clocks::init_clocks_and_plls, timer::Alarm, watchdog::Watchdog, Sio}, XOSC_CRYSTAL_FREQ, }; const BUF_SIZE: usize = 64; @@ -117,8 +111,8 @@ mod app { &mut resets, &mut watchdog, ) - .ok() - .unwrap(); + .ok() + .unwrap(); let sio = Sio::new(c.device.SIO); let pins = rp_pico::Pins::new( @@ -195,7 +189,7 @@ mod app { serial, uart, timer, - alarm + alarm, }, Local { usb_producer, @@ -204,7 +198,7 @@ mod app { uart_consumer, usb_device, }, - init::Monotonics() + init::Monotonics(), ) } @@ -272,13 +266,13 @@ mod app { if usb_device.poll(&mut [serial]) { match serial.read(&mut buf) { Ok(count) => { - buf.iter().take(count).for_each(|b| - uart_producer.enqueue(*b).ok().unwrap() - ); + buf.iter() + .take(count) + .for_each(|b| uart_producer.enqueue(*b).ok().unwrap()); uart_write::spawn().unwrap(); count } - _ => 0 + _ => 0, } } else { 0 diff --git a/boards/rp-pico/examples/pico_usb_uart_passthrough.rs b/boards/rp-pico/examples/pico_usb_uart_passthrough.rs deleted file mode 100644 index 57502735a..000000000 --- a/boards/rp-pico/examples/pico_usb_uart_passthrough.rs +++ /dev/null @@ -1,330 +0,0 @@ -//! # Pico USB Serial (with Interrupts) Example -//! -//! Creates a USB Serial device on a Pico board, with the USB driver running in -//! the USB interrupt. -//! -//! This will create a USB Serial device echoing anything it receives. Incoming -//! ASCII characters are converted to upercase, so you can tell it is working -//! and not just local-echo! -//! -//! See the `Cargo.toml` file for Copyright and license details. - -#![no_std] -#![no_main] - -// The macro for our start-up function -use rp_pico::entry; - -// The macro for marking our interrupt functions -use rp_pico::hal::pac::interrupt; - -// GPIO traits -use fugit::RateExtU32; - -// Ensure we halt the program on panic (if we don't mention this crate it won't -// be linked) -use panic_halt as _; - -// Pull in any important traits -use rp_pico::hal::prelude::*; - -// A shorter alias for the Peripheral Access Crate, which provides low-level -// register access -use rp_pico::hal::pac; - -// A shorter alias for the Hardware Abstraction Layer, which provides -// higher-level drivers. -use rp_pico::hal; - -// USB Device support -use usb_device::{class_prelude::*, prelude::*}; - -// USB Communications Class Device support -use usbd_serial::SerialPort; - - -/// Import the GPIO pins we use -use hal::gpio::pin::bank0::{Gpio0, Gpio1}; - -// UART related types -use hal::uart::{DataBits, StopBits, UartConfig}; - -// These are the traits we need from Embedded HAL to treat our hardware -// objects as generic embedded devices. -use embedded_hal::{ - digital::v2::OutputPin, - serial::{Read, Write}, -}; - -/// Alias the type for our UART pins to make things clearer. -type UartPins = ( - hal::gpio::Pin>, - hal::gpio::Pin>, -); - -/// Alias the type for our UART to make things clearer. -type Uart = hal::uart::UartPeripheral; - -/// The USB Device Driver (shared with the interrupt). -static mut USB_DEVICE: Option> = None; - -/// The USB Bus Driver (shared with the interrupt). -static mut USB_BUS: Option> = None; - -/// The USB Serial Device Driver (shared with the interrupt). -static mut USB_SERIAL: Option> = None; - -/// This how we transfer the UART into the Interrupt Handler -static mut GLOBAL_UART: Option = None; - -static mut USB_WRITE_BUF: [u8; 1024] = [0u8; 1024]; - -/// Entry point to our bare-metal application. -/// -/// The `#[entry]` macro ensures the Cortex-M start-up code calls this function -/// as soon as all global variables are initialised. -/// -/// The function configures the RP2040 peripherals, then blinks the LED in an -/// infinite loop. -#[entry] -fn main() -> ! { - // Grab our singleton objects - let mut pac = pac::Peripherals::take().unwrap(); - let core = pac::CorePeripherals::take().unwrap(); - - // Set up the watchdog driver - needed by the clock setup code - let mut watchdog = hal::Watchdog::new(pac.WATCHDOG); - - // Configure the clocks - // - // The default is to generate a 125 MHz system clock - let clocks = hal::clocks::init_clocks_and_plls( - rp_pico::XOSC_CRYSTAL_FREQ, - pac.XOSC, - pac.CLOCKS, - pac.PLL_SYS, - pac.PLL_USB, - &mut pac.RESETS, - &mut watchdog, - ) - .ok() - .unwrap(); - - // Set up the USB driver - let usb_bus = UsbBusAllocator::new(hal::usb::UsbBus::new( - pac.USBCTRL_REGS, - pac.USBCTRL_DPRAM, - clocks.usb_clock, - true, - &mut pac.RESETS, - )); - unsafe { - // Note (safety): This is safe as interrupts haven't been started yet - USB_BUS = Some(usb_bus); - } - - // Grab a reference to the USB Bus allocator. We are promising to the - // compiler not to take mutable access to this global variable whilst this - // reference exists! - let bus_ref = unsafe { USB_BUS.as_ref().unwrap() }; - - // Set up the USB Communications Class Device driver - let serial = SerialPort::new(bus_ref); - unsafe { - USB_SERIAL = Some(serial); - } - - // Create a USB device with a fake VID and PID - let usb_dev = UsbDeviceBuilder::new(bus_ref, UsbVidPid(0x16c0, 0x27dd)) - .manufacturer("Fake company") - .product("Serial port") - .serial_number("MJVN") - .device_class(2) // from: https://www.usb.org/defined-class-codes - .build(); - unsafe { - // Note (safety): This is safe as interrupts haven't been started yet - USB_DEVICE = Some(usb_dev); - } - - // Enable the USB interrupt - unsafe { - pac::NVIC::unmask(hal::pac::Interrupt::USBCTRL_IRQ); - }; - - // No more USB code after this point in main! We can do anything we want in - // here since USB is handled in the interrupt - let's blink an LED! - - // The delay object lets us wait for specified amounts of time (in - // milliseconds) - let mut delay = cortex_m::delay::Delay::new(core.SYST, clocks.system_clock.freq().to_Hz()); - - // The single-cycle I/O block controls our GPIO pins - let sio = hal::Sio::new(pac.SIO); - - // Set the pins up according to their function on this particular board - let pins = rp_pico::Pins::new( - pac.IO_BANK0, - pac.PADS_BANK0, - sio.gpio_bank0, - &mut pac.RESETS, - ); - - - let uart_pins = ( - // UART TX (characters sent from RP2040) on pin 1 (GPIO0) - pins.gpio0.into_mode::(), - // UART RX (characters received by RP2040) on pin 2 (GPIO1) - pins.gpio1.into_mode::(), - ); - - // Make a UART on the given pins - let mut uart = hal::uart::UartPeripheral::new(pac.UART0, uart_pins, &mut pac.RESETS) - .enable( - UartConfig::new(115200.Hz(), DataBits::Eight, None, StopBits::One), - clocks.peripheral_clock.freq(), - ) - .unwrap(); - - // Tell the UART to raise its interrupt line on the NVIC when the RX FIFO - // has data in it. - uart.enable_rx_interrupt(); - - unsafe { - // Note (safety): This is safe as interrupts haven't been started yet - GLOBAL_UART = Some(uart); - } - - unsafe { - // Enable the UART interrupt in the *Nested Vectored Interrupt - // Controller*, which is part of the Cortex-M0+ core. - pac::NVIC::unmask(hal::pac::Interrupt::UART0_IRQ); - } - - - // Set the LED to be an output - let mut led_pin = pins.led.into_push_pull_output(); - - // Blink the LED at 1 Hz - loop { - led_pin.set_high().unwrap(); - delay.delay_ms(500); - led_pin.set_low().unwrap(); - delay.delay_ms(500); - } - // Ok(()) -} - -/// This function is called whenever the USB Hardware generates an Interrupt -/// Request. -/// -/// We do all our USB work under interrupt, so the main thread can continue on -/// knowing nothing about USB. -#[allow(non_snake_case)] -#[interrupt] -unsafe fn USBCTRL_IRQ() { - use core::sync::atomic::{AtomicBool, Ordering}; - - // Disable the USB interrupt - // unsafe { - // pac::NVIC::mask(hal::pac::Interrupt::USBCTRL_IRQ); - // }; - - /// Note whether we've already printed the "hello" message. - static SAID_HELLO: AtomicBool = AtomicBool::new(false); - - // Grab the global objects. This is OK as we only access them under interrupt. - let usb_dev = USB_DEVICE.as_mut().unwrap(); - let serial = USB_SERIAL.as_mut().unwrap(); - let uart = GLOBAL_UART.as_mut().unwrap(); - // let mut buf = USB_WRITE_BUF.as_mut().unwrap(); - - // Say hello exactly once on start-up - // if !SAID_HELLO.load(Ordering::Relaxed) { - // SAID_HELLO.store(true, Ordering::Relaxed); - // let _ = serial.write(b"Hello, World!\r\n"); - // } - - // Poll the USB driver with all of our supported USB Classes - if usb_dev.poll(&mut [serial]) { - - loop { - match serial.read(&mut USB_WRITE_BUF) { - Err(_e) => { - // Do nothing - break; - } - Ok(0) => { - // Do nothing - break; - } - Ok(count) => { - // Convert to upper case - USB_WRITE_BUF.iter().take(count).for_each(|b| { - uart.write(*b); - }); - } - } - } - } - - // Enable the USB interrupt - // unsafe { - // pac::NVIC::unmask(hal::pac::Interrupt::USBCTRL_IRQ); - // }; -} - -#[interrupt] -unsafe fn UART0_IRQ() { - // Disable the USB interrupt - // unsafe { - // pac::NVIC::mask(hal::pac::Interrupt::UART0_IRQ); - // }; - let uart = GLOBAL_UART.as_mut().unwrap(); - let serial = USB_SERIAL.as_mut().unwrap(); - // Echo the input back to the output until the FIFO is empty. Reading - // from the UART should also clear the UART interrupt flag. - let mut buf = [0u8; 32]; - let mut ptr = 0; - while let Ok(byte) = uart.read() { - buf[ptr] = byte; - if ptr + 1 >= buf.len() { - let mut write_ptr = 0; - loop { - let mut data = &buf[write_ptr..ptr]; - let len = serial.write(data); - if let Ok(len) = len { - write_ptr += len; - if write_ptr < ptr { - continue; - } - } - break; - } - ptr = 0; - } else { - ptr += 1; - } - } - if ptr > 0 { - let mut write_ptr = 0; - loop { - let mut data = &buf[write_ptr..ptr]; - let len = serial.write(data); - if let Ok(len) = len { - write_ptr += len; - if write_ptr < ptr { - continue; - } - } - break; - } - } - // Enable the UART interrupt - // unsafe { - // pac::NVIC::unmask(hal::pac::Interrupt::UART0_IRQ); - // }; - // Set an event to ensure the main thread always wakes up, even if it's in - // the process of going to sleep. - cortex_m::asm::sev(); -} -// End of file