From d37ae696ffd347ec4a177f492cdc7f0f6b5cd134 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?D=C3=A1niel=20Buga?= Date: Wed, 1 May 2024 22:38:59 +0200 Subject: [PATCH] embassy-usb support (#1517) * embassy-usb support * Add changelog entry * Update embassy-usb-synopsys-otg * Change VID/PID to match the blocking example * Add missing initialisation * Clean up * fmt * Remove log init * Use released crate * Revert to released embassy-usb * Update vid/pid * Remove redundant TAIT feature gate --- esp-hal/CHANGELOG.md | 1 + esp-hal/Cargo.toml | 79 ++++--- esp-hal/src/otg_fs.rs | 304 ++++++++++++++++++++++--- examples/Cargo.toml | 8 +- examples/src/bin/embassy_usb_serial.rs | 131 +++++++++++ examples/src/bin/usb_serial.rs | 2 +- 6 files changed, 455 insertions(+), 70 deletions(-) create mode 100644 examples/src/bin/embassy_usb_serial.rs diff --git a/esp-hal/CHANGELOG.md b/esp-hal/CHANGELOG.md index e513aee2194..20c1b41f7a7 100644 --- a/esp-hal/CHANGELOG.md +++ b/esp-hal/CHANGELOG.md @@ -14,6 +14,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - ESP32-PICO-V3-02: Initial support (#1155) - `time::current_time` API (#1503) - ESP32-S3: Add LCD_CAM Camera driver (#1483) +- `embassy-usb` support (#1517) ### Fixed diff --git a/esp-hal/Cargo.toml b/esp-hal/Cargo.toml index d098075143d..c6b7608c53b 100644 --- a/esp-hal/Cargo.toml +++ b/esp-hal/Cargo.toml @@ -15,39 +15,41 @@ features = ["embedded-hal", "esp32c6"] rustdoc-args = ["--cfg", "docsrs"] [dependencies] -bitflags = "2.5.0" -bitfield = "0.15.0" -cfg-if = "1.0.0" -critical-section = "1.1.2" -defmt = { version = "0.3.6", optional = true } -document-features = "0.2.8" -embassy-executor = { version = "0.5.0", optional = true } -embassy-futures = { version = "0.1.1", optional = true } -embassy-sync = { version = "0.5.0", optional = true } -embassy-time-driver = { version = "0.1.0", optional = true } -embedded-can = { version = "0.4.1", optional = true } -embedded-dma = "0.2.0" -embedded-hal-02 = { version = "0.2.7", optional = true, features = ["unproven"], package = "embedded-hal" } -embedded-hal = { version = "1.0.0", optional = true } -embedded-hal-async = { version = "1.0.0", optional = true } -embedded-hal-nb = { version = "1.0.0", optional = true } -embedded-io = { version = "0.6.1", optional = true } -embedded-io-async = { version = "0.6.1", optional = true } -enumset = "1.1.3" -esp-synopsys-usb-otg = { version = "0.4.1", optional = true, features = ["fs", "esp32sx"] } -fugit = "0.3.7" -log = { version = "0.4.21", optional = true } -nb = "1.1.0" -paste = "1.0.14" -portable-atomic = { version = "1.6.0", default-features = false } -procmacros = { version = "0.10.0", features = ["enum-dispatch", "interrupt", "ram"], package = "esp-hal-procmacros", path = "../esp-hal-procmacros" } -riscv = { version = "0.11.1", optional = true } -strum = { version = "0.26.2", default-features = false, features = ["derive"] } -void = { version = "1.0.2", default-features = false } -usb-device = { version = "0.3.2", optional = true } -rand_core = "0.6.4" -ufmt-write = { version = "0.1.0", optional = true } -xtensa-lx = { version = "0.9.0", optional = true } +bitflags = "2.5.0" +bitfield = "0.15.0" +cfg-if = "1.0.0" +critical-section = "1.1.2" +defmt = { version = "0.3.6", optional = true } +document-features = "0.2.8" +embassy-executor = { version = "0.5.0", optional = true } +embassy-futures = { version = "0.1.1", optional = true } +embassy-sync = { version = "0.5.0", optional = true } +embassy-time-driver = { version = "0.1.0", optional = true } +embassy-usb-driver = { version = "0.1.0", optional = true } +embassy-usb-synopsys-otg = { version = "0.1.0", optional = true } +embedded-can = { version = "0.4.1", optional = true } +embedded-dma = "0.2.0" +embedded-hal-02 = { version = "0.2.7", optional = true, features = ["unproven"], package = "embedded-hal" } +embedded-hal = { version = "1.0.0", optional = true } +embedded-hal-async = { version = "1.0.0", optional = true } +embedded-hal-nb = { version = "1.0.0", optional = true } +embedded-io = { version = "0.6.1", optional = true } +embedded-io-async = { version = "0.6.1", optional = true } +enumset = "1.1.3" +esp-synopsys-usb-otg = { version = "0.4.1", optional = true, features = ["fs", "esp32sx"] } +fugit = "0.3.7" +log = { version = "0.4.21", optional = true } +nb = "1.1.0" +paste = "1.0.14" +portable-atomic = { version = "1.6.0", default-features = false } +procmacros = { version = "0.10.0", features = ["enum-dispatch", "interrupt", "ram"], package = "esp-hal-procmacros", path = "../esp-hal-procmacros" } +riscv = { version = "0.11.1", optional = true } +strum = { version = "0.26.2", default-features = false, features = ["derive"] } +void = { version = "1.0.2", default-features = false } +usb-device = { version = "0.3.2", optional = true } +rand_core = "0.6.4" +ufmt-write = { version = "0.1.0", optional = true } +xtensa-lx = { version = "0.9.0", optional = true } # IMPORTANT: # Each supported device MUST have its PAC included below along with a @@ -128,13 +130,16 @@ rv-init-rtc-data = ["esp-riscv-rt/init-rtc-fast-data", "esp-riscv-rt/init-rtc-fa #! ### Trait Implementation Feature Flags ## Enable support for asynchronous operation, with interfaces provided by ## `embedded-hal-async` and `embedded-io-async`. +## Also enables `embassy-usb` support for ESP32-S2 and ESP32-S3. async = [ "embedded-hal", "embedded-hal-async", - "embassy-sync", - "embassy-futures", "embedded-io", "embedded-io-async", + "embassy-sync", + "embassy-futures", + "embassy-usb-driver", + "embassy-usb-synopsys-otg" ] ## Implement `defmt::Format` on certain types. defmt = [ @@ -171,11 +176,11 @@ embassy = ["embassy-time-driver", "procmacros/embassy", "embassy-executor"] ## by the time driver. embassy-integrated-timers = ["embassy-executor?/integrated-timers"] ## Enable the embassy time driver using the `SYSTIMER` peripheral. The -## `SYSTIMER` peripheral has three alarams available for use. Do **not** +## `SYSTIMER` peripheral has three alarams available for use. Do **not** ## use when targeting an `esp32s2`. embassy-time-systick-16mhz = ["embassy-time-driver/tick-hz-16_000_000"] ## Enable the embassy time driver using the `SYSTIMER` peripheral. The -## `SYSTIMER` peripheral has three alarams available for use. Must only +## `SYSTIMER` peripheral has three alarams available for use. Must only ## be used when targeting an `esp32s2`. embassy-time-systick-80mhz = ["embassy-time-driver/tick-hz-80_000_000"] ## Enable the embassy time driver using the `TIMG0` peripheral. The `TIMG0` diff --git a/esp-hal/src/otg_fs.rs b/esp-hal/src/otg_fs.rs index 49883df17ad..91d9b191b0e 100644 --- a/esp-hal/src/otg_fs.rs +++ b/esp-hal/src/otg_fs.rs @@ -3,28 +3,26 @@ //! ## Overview //! The USB OTG Full-speed peripheral driver provides support for the USB //! On-The-Go (OTG) full-speed functionality on ESP chips, allows communication -//! with USB devices. +//! with USB devices using either blocking (usb-device) or asynchronous +//! (embassy-usb) APIs. //! -//! The driver uses the `esp_synopsys_usb_otg` crate, which provides the `USB -//! bus` implementation and `USB peripheral traits`. It also relies on other -//! peripheral modules, such as `GPIO`, `system`, and `clock control`, to -//! configure and enable the `USB` peripheral. +//! The blocking driver uses the `esp_synopsys_usb_otg` crate, which provides +//! the `USB bus` implementation and `USB peripheral traits`. //! -//! To use the USB OTG Full-speed peripheral driver, you need to initialize the -//! peripheral and configure its settings. The USB struct represents the USB -//! peripheral and requires the implementation of the `UsbSel`, `UsbDp`, and -//! `UsbDm` traits, which define the specific types used for USB pin selection -//! and data pins. +//! The asynchronous driver uses the `embassy_usb_synopsys_otg` crate, which +//! provides the `USB bus` and `USB device` implementations. +//! +//! The module also relies on other peripheral modules, such as `GPIO`, +//! `system`, and `clock control`, to configure and enable the `USB` peripheral. //! -//! The USB struct provides a `new` function for initialization. -//! Inside the `new` function, the `into_ref!` macro is used to convert the -//! peripheral references into `PeripheralRef` instances. +//! To use the USB OTG Full-speed peripheral driver, you need to initialize the +//! peripheral and configure its settings. The [`Usb`] struct represents the USB +//! peripheral and requires the GPIO pins that implement [`UsbDp`], and +//! [`UsbDm`], which define the specific types used for USB pin selection. //! -//! The `USB` struct implements the `UsbPeripheral` trait from the -//! `esp_synopsys_usb_otg` crate, which defines the required constants and -//! functions for `USB peripheral` operation. The trait implementation includes -//! enabling the `USB peripheral`, configuring the `USB` settings and connecting -//! the appropriate `GPIO` pins to the `USB peripheral`. +//! The returned `Usb` instance can be used with the `usb-device` crate, or it +//! can be further configured with [`asynch::Driver`] to be used with the +//! `embassy-usb` crate. pub use esp_synopsys_usb_otg::UsbBus; use esp_synopsys_usb_otg::UsbPeripheral; @@ -62,18 +60,8 @@ impl<'d> Usb<'d> { _usb0: usb0.into_ref(), } } -} - -unsafe impl<'d> Sync for Usb<'d> {} - -unsafe impl<'d> UsbPeripheral for Usb<'d> { - const REGISTERS: *const () = peripherals::USB0::ptr() as *const (); - - const HIGH_SPEED: bool = false; - const FIFO_DEPTH_WORDS: usize = 256; - const ENDPOINT_COUNT: usize = 5; - fn enable() { + fn _enable() { unsafe { let usb_wrap = &*peripherals::USB_WRAP::PTR; usb_wrap.otg_conf().modify(|_, w| { @@ -103,8 +91,266 @@ unsafe impl<'d> UsbPeripheral for Usb<'d> { } } + fn _disable() { + // TODO + } +} + +unsafe impl<'d> Sync for Usb<'d> {} + +unsafe impl<'d> UsbPeripheral for Usb<'d> { + const REGISTERS: *const () = peripherals::USB0::ptr() as *const (); + + const HIGH_SPEED: bool = false; + const FIFO_DEPTH_WORDS: usize = 256; + const ENDPOINT_COUNT: usize = 5; + + fn enable() { + Self::_enable(); + } + fn ahb_frequency_hz(&self) -> u32 { // unused 80_000_000 } } + +#[cfg(feature = "async")] +pub mod asynch { + use embassy_usb_driver::{ + EndpointAddress, + EndpointAllocError, + EndpointType, + Event, + Unsupported, + }; + pub use embassy_usb_synopsys_otg::Config; + use embassy_usb_synopsys_otg::{ + on_interrupt, + otg_v1::Otg, + Bus as OtgBus, + ControlPipe, + Driver as OtgDriver, + Endpoint, + In, + OtgInstance, + Out, + PhyType, + State, + }; + use procmacros::handler; + + use super::*; + use crate::Cpu; + + // From ESP32-S3 TRM: + // Six additional endpoints (endpoint numbers 1 to 6), configurable as IN or OUT + const MAX_EP_COUNT: usize = 7; + + static STATE: State = State::new(); + + /// Asynchronous USB driver. + pub struct Driver<'d> { + inner: OtgDriver<'d, MAX_EP_COUNT>, + } + + impl<'d> Driver<'d> { + const REGISTERS: Otg = unsafe { Otg::from_ptr(Usb::REGISTERS.cast_mut()) }; + + /// Initializes USB OTG peripheral with internal Full-Speed PHY, for + /// asynchronous operation. + /// + /// # Arguments + /// + /// * `ep_out_buffer` - An internal buffer used to temporarily store + /// received packets. + /// Must be large enough to fit all OUT endpoint max packet sizes. + /// Endpoint allocation will fail if it is too small. + pub fn new(_peri: Usb<'d>, ep_out_buffer: &'d mut [u8], config: Config) -> Self { + // From `synopsys-usb-otg` crate: + // This calculation doesn't correspond to one in a Reference Manual. + // In fact, the required number of words is higher than indicated in RM. + // The following numbers are pessimistic and were figured out empirically. + const RX_FIFO_EXTRA_SIZE_WORDS: u16 = 30; + + let instance = OtgInstance { + regs: Self::REGISTERS, + state: &STATE, + fifo_depth_words: Usb::FIFO_DEPTH_WORDS as u16, + extra_rx_fifo_words: RX_FIFO_EXTRA_SIZE_WORDS, + endpoint_count: Usb::ENDPOINT_COUNT, + phy_type: PhyType::InternalFullSpeed, + quirk_setup_late_cnak: quirk_setup_late_cnak(), + calculate_trdt_fn: |_| 5, + }; + Self { + inner: OtgDriver::new(ep_out_buffer, instance, config), + } + } + } + + impl<'d> embassy_usb_driver::Driver<'d> for Driver<'d> { + type EndpointOut = Endpoint<'d, Out>; + type EndpointIn = Endpoint<'d, In>; + type ControlPipe = ControlPipe<'d>; + type Bus = Bus<'d>; + + fn alloc_endpoint_in( + &mut self, + ep_type: EndpointType, + max_packet_size: u16, + interval_ms: u8, + ) -> Result { + self.inner + .alloc_endpoint_in(ep_type, max_packet_size, interval_ms) + } + + fn alloc_endpoint_out( + &mut self, + ep_type: EndpointType, + max_packet_size: u16, + interval_ms: u8, + ) -> Result { + self.inner + .alloc_endpoint_out(ep_type, max_packet_size, interval_ms) + } + + fn start(self, control_max_packet_size: u16) -> (Self::Bus, Self::ControlPipe) { + let (bus, cp) = self.inner.start(control_max_packet_size); + + ( + Bus { + inner: bus, + inited: false, + }, + cp, + ) + } + } + + /// Asynchronous USB bus mainly used internally by `embassy-usb`. + // We need a custom wrapper implementation to handle custom initialization. + pub struct Bus<'d> { + inner: OtgBus<'d, MAX_EP_COUNT>, + inited: bool, + } + + impl<'d> Bus<'d> { + fn init(&mut self) { + Usb::_enable(); + + let r = Driver::REGISTERS; + + // Wait for AHB ready. + while !r.grstctl().read().ahbidl() {} + + // Configure as device. + r.gusbcfg().write(|w| { + // Force device mode + w.set_fdmod(true); + w.set_srpcap(false); + // Enable internal full-speed PHY + w.set_physel(true); + }); + self.inner.config_v1(); + + // Perform core soft-reset + r.grstctl().modify(|w| w.set_csrst(true)); + while r.grstctl().read().csrst() {} + + r.pcgcctl().modify(|w| { + // Disable power down + w.set_stppclk(false); + }); + + unsafe { + crate::interrupt::bind_interrupt( + crate::peripherals::Interrupt::USB, + interrupt_handler.handler(), + ); + crate::interrupt::enable( + crate::peripherals::Interrupt::USB, + interrupt_handler.priority(), + ) + .unwrap(); + } + } + + fn disable(&mut self) { + crate::interrupt::disable(Cpu::ProCpu, crate::peripherals::Interrupt::USB); + + #[cfg(multi_core)] + crate::interrupt::disable(Cpu::AppCpu, crate::peripherals::Interrupt::USB); + + Usb::_disable(); + } + } + + impl<'d> embassy_usb_driver::Bus for Bus<'d> { + async fn poll(&mut self) -> Event { + if !self.inited { + self.init(); + self.inited = true; + } + + self.inner.poll().await + } + + fn endpoint_set_stalled(&mut self, ep_addr: EndpointAddress, stalled: bool) { + self.inner.endpoint_set_stalled(ep_addr, stalled) + } + + fn endpoint_is_stalled(&mut self, ep_addr: EndpointAddress) -> bool { + self.inner.endpoint_is_stalled(ep_addr) + } + + fn endpoint_set_enabled(&mut self, ep_addr: EndpointAddress, enabled: bool) { + self.inner.endpoint_set_enabled(ep_addr, enabled) + } + + async fn enable(&mut self) { + self.inner.enable().await + } + + async fn disable(&mut self) { + self.inner.disable().await + } + + async fn remote_wakeup(&mut self) -> Result<(), Unsupported> { + self.inner.remote_wakeup().await + } + } + + impl<'d> Drop for Bus<'d> { + fn drop(&mut self) { + Bus::disable(self); + } + } + + fn quirk_setup_late_cnak() -> bool { + // Our CID register is 4 bytes offset from what's in embassy-usb-synopsys-otg + let cid = unsafe { + Driver::REGISTERS + .as_ptr() + .cast::() + .add(0x40) + .read_volatile() + }; + // ESP32-Sx has a different CID register value, too + cid == 0x4f54_400a || cid & 0xf000 == 0x1000 + } + + #[handler(priority = crate::interrupt::Priority::max())] + fn interrupt_handler() { + let setup_late_cnak = quirk_setup_late_cnak(); + + unsafe { + on_interrupt( + Driver::REGISTERS, + &STATE, + Usb::ENDPOINT_COUNT, + setup_late_cnak, + ) + } + } +} diff --git a/examples/Cargo.toml b/examples/Cargo.toml index 4db7a5f56dc..14b02c74fdf 100644 --- a/examples/Cargo.toml +++ b/examples/Cargo.toml @@ -14,8 +14,10 @@ elliptic-curve = { version = "0.13.8", default-features = false, features = # some examples use a huge amount of stack embassy-executor = { version = "0.5.0", features = ["task-arena-size-40960"] } embassy-sync = "0.5.0" +embassy-futures = "0.1.1" embassy-time = "0.3.0" embassy-time-driver = { version = "0.1.0", optional = true } +embassy-usb = { version = "0.1.0", default-features = false, optional = true } embedded-graphics = "0.8.1" embedded-hal = "1.0.0" embedded-hal-02 = { version = "0.2.7", package = "embedded-hal", features = ["unproven"] } @@ -36,8 +38,8 @@ lis3dh-async = "0.9.2" nb = "1.1.0" p192 = { version = "0.13.0", default-features = false, features = ["arithmetic"] } p256 = { version = "0.13.2", default-features = false, features = ["arithmetic"] } -portable-atomic = { version = "1.6.0", default-features = false } -sha2 = { version = "0.10.8", default-features = false} +portable-atomic = { version = "1.6.0", default-features = false } +sha2 = { version = "0.10.8", default-features = false } smart-leds = "0.4.0" ssd1306 = "0.8.4" static_cell = { version = "2.0.0", features = ["nightly"] } @@ -54,7 +56,7 @@ esp32h2 = ["esp-hal/esp32h2", "esp-backtrace/esp32h2", "esp-println/esp32h2", "e esp32s2 = ["esp-hal/esp32s2", "esp-backtrace/esp32s2", "esp-println/esp32s2", "esp-hal-smartled/esp32s2"] esp32s3 = ["esp-hal/esp32s3", "esp-backtrace/esp32s3", "esp-println/esp32s3", "esp-hal-smartled/esp32s3"] -async = ["esp-hal/async"] +async = ["esp-hal/async", "embassy-usb"] embedded-hal-02 = ["esp-hal/embedded-hal-02"] embedded-hal = ["esp-hal/embedded-hal"] diff --git a/examples/src/bin/embassy_usb_serial.rs b/examples/src/bin/embassy_usb_serial.rs new file mode 100644 index 00000000000..517b879dca8 --- /dev/null +++ b/examples/src/bin/embassy_usb_serial.rs @@ -0,0 +1,131 @@ +//! CDC-ACM serial port example using embassy. +//! +//! This example should be built in release mode. + +//% CHIPS: esp32s2 esp32s3 +//% FEATURES: async embassy embassy-executor-thread embassy-time-timg0 embassy-generic-timers + +#![no_std] +#![no_main] + +use embassy_executor::Spawner; +use embassy_futures::join::join; +use embassy_usb::{ + class::cdc_acm::{CdcAcmClass, State}, + driver::EndpointError, + Builder, +}; +use esp_backtrace as _; +use esp_hal::{ + clock::ClockControl, + embassy, + gpio::Io, + otg_fs::{ + asynch::{Config, Driver}, + Usb, + }, + peripherals::Peripherals, + prelude::*, + system::SystemControl, + timer::TimerGroup, +}; + +#[main] +async fn main(_spawner: Spawner) -> () { + esp_println::println!("Init!"); + let peripherals = Peripherals::take(); + let system = SystemControl::new(peripherals.SYSTEM); + let clocks = ClockControl::boot_defaults(system.clock_control).freeze(); + + embassy::init(&clocks, TimerGroup::new_async(peripherals.TIMG0, &clocks)); + let io = Io::new(peripherals.GPIO, peripherals.IO_MUX); + + let usb = Usb::new(peripherals.USB0, io.pins.gpio19, io.pins.gpio20); + + // Create the driver, from the HAL. + let mut ep_out_buffer = [0u8; 1024]; + let config = Config::default(); + + let driver = Driver::new(usb, &mut ep_out_buffer, config); + + // Create embassy-usb Config + let mut config = embassy_usb::Config::new(0x303A, 0x3001); + config.manufacturer = Some("Espressif"); + config.product = Some("USB-serial example"); + config.serial_number = Some("12345678"); + + // Required for windows compatibility. + // https://developer.nordicsemi.com/nRF_Connect_SDK/doc/1.9.1/kconfig/CONFIG_CDC_ACM_IAD.html#help + config.device_class = 0xEF; + config.device_sub_class = 0x02; + config.device_protocol = 0x01; + config.composite_with_iads = true; + + // Create embassy-usb DeviceBuilder using the driver and config. + // It needs some buffers for building the descriptors. + let mut device_descriptor = [0; 256]; + let mut config_descriptor = [0; 256]; + let mut bos_descriptor = [0; 256]; + let mut control_buf = [0; 64]; + + let mut state = State::new(); + + let mut builder = Builder::new( + driver, + config, + &mut device_descriptor, + &mut config_descriptor, + &mut bos_descriptor, + &mut [], // no msos descriptors + &mut control_buf, + ); + + // Create classes on the builder. + let mut class = CdcAcmClass::new(&mut builder, &mut state, 64); + + // Build the builder. + let mut usb = builder.build(); + + // Run the USB device. + let usb_fut = usb.run(); + + // Do stuff with the class! + let echo_fut = async { + loop { + class.wait_connection().await; + esp_println::println!("Connected"); + let _ = echo(&mut class).await; + esp_println::println!("Disconnected"); + } + }; + + // Run everything concurrently. + // If we had made everything `'static` above instead, we could do this using separate tasks instead. + join(usb_fut, echo_fut).await; +} + +async fn echo<'d>(class: &mut CdcAcmClass<'d, Driver<'d>>) -> Result<(), Disconnected> { + let mut buf = [0; 64]; + loop { + let n = class.read_packet(&mut buf).await?; + // Echo back in upper case + for c in buf[0..n].iter_mut() { + if 0x61 <= *c && *c <= 0x7a { + *c &= !0x20; + } + } + let data = &buf[..n]; + class.write_packet(data).await?; + } +} + +struct Disconnected {} + +impl From for Disconnected { + fn from(val: EndpointError) -> Self { + match val { + EndpointError::BufferOverflow => panic!("Buffer overflow"), + EndpointError::Disabled => Disconnected {}, + } + } +} diff --git a/examples/src/bin/usb_serial.rs b/examples/src/bin/usb_serial.rs index 6ee4b894eff..0bfb2208d7b 100644 --- a/examples/src/bin/usb_serial.rs +++ b/examples/src/bin/usb_serial.rs @@ -32,7 +32,7 @@ fn main() -> ! { let mut serial = SerialPort::new(&usb_bus); - let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x16c0, 0x27dd)) + let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x303A, 0x3001)) .device_class(USB_CLASS_CDC) .build();