diff --git a/embassy-rp/src/usb.rs b/embassy-rp/src/usb/device.rs similarity index 95% rename from embassy-rp/src/usb.rs rename to embassy-rp/src/usb/device.rs index 96541ade67..1b8f982401 100644 --- a/embassy-rp/src/usb.rs +++ b/embassy-rp/src/usb/device.rs @@ -1,10 +1,9 @@ -//! USB driver. use core::future::poll_fn; use core::marker::PhantomData; use core::slice; -use core::sync::atomic::{compiler_fence, Ordering}; use core::task::Poll; +use atomic_polyfill::{compiler_fence, Ordering}; use embassy_hal_internal::PeripheralType; use embassy_sync::waitqueue::AtomicWaker; use embassy_usb_driver as driver; @@ -12,33 +11,9 @@ use embassy_usb_driver::{ Direction, EndpointAddress, EndpointAllocError, EndpointError, EndpointInfo, EndpointType, Event, Unsupported, }; +use super::{Dir, In, Instance, Out}; use crate::interrupt::typelevel::{Binding, Interrupt}; -use crate::{interrupt, pac, peripherals, Peri, RegExt}; - -trait SealedInstance { - fn regs() -> crate::pac::usb::Usb; - fn dpram() -> crate::pac::usb_dpram::UsbDpram; -} - -/// USB peripheral instance. -#[allow(private_bounds)] -pub trait Instance: SealedInstance + PeripheralType + 'static { - /// Interrupt for this peripheral. - type Interrupt: interrupt::typelevel::Interrupt; -} - -impl crate::usb::SealedInstance for peripherals::USB { - fn regs() -> pac::usb::Usb { - pac::USB - } - fn dpram() -> crate::pac::usb_dpram::UsbDpram { - pac::USB_DPRAM - } -} - -impl crate::usb::Instance for peripherals::USB { - type Interrupt = crate::interrupt::typelevel::USBCTRL_IRQ; -} +use crate::{interrupt, pac, Peri, RegExt}; const EP_COUNT: usize = 16; const EP_MEMORY_SIZE: usize = 4096; @@ -99,14 +74,14 @@ impl EndpointData { } /// RP2040 USB driver handle. -pub struct Driver<'d, T: Instance> { +pub struct Driver<'d, T: Instance + PeripheralType> { phantom: PhantomData<&'d mut T>, ep_in: [EndpointData; EP_COUNT], ep_out: [EndpointData; EP_COUNT], ep_mem_free: u16, // first free address in EP mem, in bytes. } -impl<'d, T: Instance> Driver<'d, T> { +impl<'d, T: Instance + PeripheralType> Driver<'d, T> { /// Create a new USB driver. pub fn new(_usb: Peri<'d, T>, _irq: impl Binding>) -> Self { T::Interrupt::unpend(); @@ -290,7 +265,7 @@ impl interrupt::typelevel::Handler for InterruptHandl } } -impl<'d, T: Instance> driver::Driver<'d> for Driver<'d, T> { +impl<'d, T: Instance + PeripheralType> driver::Driver<'d> for Driver<'d, T> { type EndpointOut = Endpoint<'d, T, Out>; type EndpointIn = Endpoint<'d, T, In>; type ControlPipe = ControlPipe<'d, T>; @@ -491,26 +466,6 @@ impl<'d, T: Instance> driver::Bus for Bus<'d, T> { } } -trait Dir { - fn dir() -> Direction; -} - -/// Type for In direction. -pub enum In {} -impl Dir for In { - fn dir() -> Direction { - Direction::In - } -} - -/// Type for Out direction. -pub enum Out {} -impl Dir for Out { - fn dir() -> Direction { - Direction::Out - } -} - /// Endpoint for RP USB driver. pub struct Endpoint<'d, T: Instance, D> { _phantom: PhantomData<(&'d mut T, D)>, diff --git a/embassy-rp/src/usb/host.rs b/embassy-rp/src/usb/host.rs new file mode 100644 index 0000000000..9964fa16dc --- /dev/null +++ b/embassy-rp/src/usb/host.rs @@ -0,0 +1,862 @@ +use core::future::poll_fn; +use core::marker::PhantomData; +use core::task::Poll; + +use atomic_polyfill::{AtomicU16, AtomicUsize, Ordering}; +use embassy_hal_internal::{Peri, PeripheralType}; +use embassy_sync::waitqueue::AtomicWaker; +use embassy_usb_driver::host::{ + channel, ChannelError, DeviceEvent, HostError, SetupPacket, TimeoutConfig, UsbChannel, UsbHostDriver, +}; +use embassy_usb_driver::{EndpointInfo, EndpointType, Speed}; +use rp_pac::usb_dpram::vals::EpControlEndpointType; + +use super::{EndpointBuffer, Instance, BUS_WAKER, DPRAM_DATA_OFFSET, EP_IN_WAKERS, EP_MEMORY}; +use crate::interrupt::typelevel::{Binding, Interrupt}; +use crate::interrupt::{self}; +use crate::usb::EP_MEMORY_SIZE; +use crate::RegExt; + +const MAIN_BUFFER_SIZE: usize = 1024; + +/// Current channel with ongoing transfer +/// +/// 0 means None +static CURRENT_CHANNEL: AtomicUsize = AtomicUsize::new(0); + +/// RP2040 USB host driver handle. +pub struct Driver<'d, T: Instance> { + phantom: PhantomData<&'d mut T>, + /// Bitset of allocated interrupt pipes + allocated_pipes: AtomicU16, + /// Index for next 'allocated' channel + channel_index: AtomicUsize, +} + +impl<'d, T: Instance> Driver<'d, T> { + /// Create a new USB driver. + pub fn new(_usb: impl PeripheralType + 'd, _irq: impl Binding>) -> Self { + let regs = T::regs(); + unsafe { + // FIXME(magic): + // zero fill regs + let p = regs.as_ptr() as *mut u32; + for i in 0..0x9c / 4 { + p.add(i).write_volatile(0) + } + + // zero fill epmem + let p = EP_MEMORY as *mut u32; + for i in 0..0x180 / 4 { + p.add(i).write_volatile(0) + } + } + + regs.usb_muxing().modify(|w| { + w.set_to_phy(true); + w.set_softcon(true); + }); + regs.usb_pwr().modify(|w| { + w.set_vbus_detect(true); + w.set_vbus_detect_override_en(true); + }); + regs.main_ctrl().modify(|w| { + w.set_controller_en(true); + w.set_host_ndevice(true); + }); + regs.sie_ctrl().modify(|w| { + w.set_sof_en(true); + w.set_keep_alive_en(true); + w.set_pulldown_en(true); + }); + + regs.inte().write(|w| { + w.set_buff_status(true); + w.set_host_resume(true); + w.set_error_data_seq(true); + w.set_error_crc(true); + w.set_error_bit_stuff(true); + }); + + T::Interrupt::unpend(); + unsafe { T::Interrupt::enable() }; + + // Initialize the bus so that it signals that power is available + BUS_WAKER.wake(); + + Self { + phantom: PhantomData, + allocated_pipes: AtomicU16::new(0), + // 1-15 are reserved for interrupt EPs + channel_index: AtomicUsize::new(16), + } + } +} + +/// USB endpoint. +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct Channel<'d, T: Instance, E, D> { + _phantom: PhantomData<(&'d mut T, E, D)>, + index: usize, + buf: EndpointBuffer, + dev_addr: u8, + + max_packet_size: u16, + ep_addr: u8, + + /// Interrupt endpoint poll interval + interval: u8, + + /// DATA0-DATA1 state + pid: bool, + /// Send PRE packet + pre: bool, +} + +impl<'d, T: Instance, E: channel::Type, D: channel::Direction> Channel<'d, T, E, D> { + /// [EP_MEMORY]-relative address + fn new(index: usize, buf_addr: u16, buf_len: u16, ep_info: &EndpointInfo, dev_addr: u8, pre: bool) -> Self { + // TODO: assert only in debug? + assert!(ep_info.ep_type == E::ep_type()); + assert!(buf_addr + buf_len <= EP_MEMORY_SIZE as u16); + assert!(ep_info.max_packet_size <= buf_len); + + // TODO: Support isochronous, bulk, and interrupt OUT + assert!(E::ep_type() != EndpointType::Isochronous); + assert!(E::ep_type() != EndpointType::Bulk); + assert!(!(E::ep_type() == EndpointType::Interrupt && D::is_out())); + + if ep_info.ep_type == EndpointType::Interrupt { + assert!(index > 0 && index < 16); + } else { + assert!(index >= 16); + } + + Self { + _phantom: PhantomData, + index, + dev_addr, + buf: EndpointBuffer { + addr: buf_addr, + len: buf_len, + _phantom: PhantomData, + }, + max_packet_size: ep_info.max_packet_size, + ep_addr: ep_info.addr.into(), + interval: ep_info.interval_ms, + pid: false, + pre, + } + } +} + +type BufferControlReg = rp_pac::common::Reg; +type EpControlReg = rp_pac::common::Reg; +type AddrControlReg = rp_pac::common::Reg; +impl<'d, T: Instance, E: channel::Type, D: channel::Direction> Channel<'d, T, E, D> { + /// Get channel waker + fn waker(&self) -> &AtomicWaker { + if Self::is_interrupt_in() { + &EP_IN_WAKERS[self.index] + } else { + &EP_IN_WAKERS[0] + } + } + + /// Get buffer control register + fn buffer_control(&self) -> BufferControlReg { + let index = if Self::is_interrupt_in() { + // Validated 1-15 + self.index + } else { + 0 + }; + T::dpram().ep_in_buffer_control(index) + } + + /// Get endpoint control register + fn ep_control(&self) -> EpControlReg { + if Self::is_interrupt_in() { + T::dpram().ep_in_control(self.index - 1) + } else { + T::dpram_epx_control() + } + } + + /// Get interrupt endpoint address control + fn addr_endp_host(&self) -> AddrControlReg { + assert!(Self::is_interrupt_in()); + T::regs().addr_endp_x(self.index - 1) + } + + fn is_interrupt_in() -> bool { + E::ep_type() == EndpointType::Interrupt && D::is_in() + } + + /// Wait for buffer to be available + /// Returns stall status + async fn wait_available(&self) -> bool { + trace!("CHANNEL {} WAIT AVAILABLE", self.index); + poll_fn(|cx| { + // Both IN and OUT endpoints use IN registers on rp2040 in host mode + self.waker().register(cx.waker()); + + let reg = self.buffer_control().read(); + + // If waiting on current tx, clear interrupts + if self.is_ready_for_transaction() { + self.clear_sie_status(); + } + + // FIXME: Stall derived from other place + match reg.available(0) { + true => Poll::Pending, + false => Poll::Ready(false), + } + }) + .await + } + + /// Is hardware configured to perform transaction with this buffer + /// Always true for INTERRUPT channel + fn is_ready_for_transaction(&self) -> bool { + if Self::is_interrupt_in() { + true + } else { + let sel = CURRENT_CHANNEL.load(Ordering::Relaxed); + sel == self.index || sel == 0 + } + } + + async fn wait_ready_for_transaction(&self) { + // Wait transfer buffer to be free + self.wait_available().await; + + trace!("CHANNEL {} WAIT READY", self.index); + // Wait for other transaction end + poll_fn(|cx| { + self.waker().register(cx.waker()); + + // Other transaction in progress + if !self.is_ready_for_transaction() { + return Poll::Pending; + } + + Poll::Ready(()) + }) + .await; + } + + // FIXME: RX Timeout with LS device on hub + /// Start transaction and wait it to be complete + async fn wait_transaction(&self) -> Result<(), ChannelError> { + assert!(!Self::is_interrupt_in()); + let regs = T::regs(); + + // Enable error and cplt interrupts + regs.inte().modify(|w| { + w.set_trans_complete(true); + w.set_stall(true); + w.set_error_rx_timeout(false); + w.set_error_rx_overflow(true); + }); + + // Start transaction + // This field should be modified separately after delay + cortex_m::asm::delay(12); + T::regs().sie_ctrl().modify(|w| { + w.set_start_trans(true); + }); + + trace!("CHANNEL {} WAIT TRANSACTION", self.index); + let res = poll_fn(|cx| { + self.waker().register(cx.waker()); + + let stat = regs.sie_status().read(); + if stat.trans_complete() { + regs.sie_status().write_clear(|w| w.set_trans_complete(true)); + return Poll::Ready(Ok(())); + } + if stat.stall_rec() { + regs.sie_status().write_clear(|w| w.set_stall_rec(true)); + return Poll::Ready(Err(ChannelError::Stall)); + } + // if stat.rx_timeout() { + // regs.sie_status().write_clear(|w| w.set_rx_timeout(true)); + // return Poll::Ready(Err(ChannelError::Timeout)) + // } + if stat.rx_overflow() { + regs.sie_status().write_clear(|w| w.set_rx_overflow(true)); + return Poll::Ready(Err(ChannelError::BufferOverflow)); + } + + Poll::Pending + }) + .await; + + res + } + + /// Mark this channel as currently used and configure endpoint type + /// + /// Call once on creation for interrupt pipe + fn set_current(&self) { + let regs = T::regs(); + trace!( + "SET CURRENT: {} CHANNEL {}: dev: {}, ep: {}, max_packet: {}, preamble: {}", + E::ep_type(), + self.index, + self.dev_addr, + self.ep_addr, + self.max_packet_size, + self.pre + ); + if Self::is_interrupt_in() { + self.ep_control().write(|w| { + w.set_endpoint_type(EpControlEndpointType::INTERRUPT); + w.set_interrupt_per_buff(true); + + // FIXME: host_poll_interval (bits 16:25) + let interval = self.interval as u32 - 1; + w.0 |= interval << 16; + + w.set_buffer_address(self.buf.addr); + w.set_enable(true); + }); + + // FIXME: What is this for? + regs.sie_ctrl().modify(|w| w.set_sof_sync(true)); + + self.addr_endp_host().write(|w| { + w.set_address(self.dev_addr); + w.set_endpoint(self.ep_addr); + // FIXME: INTERRUPT OUT? + w.set_intep_dir(D::is_out()); + w.set_intep_preamble(self.pre) + }); + } else { + CURRENT_CHANNEL.store(self.index, Ordering::Relaxed); + + T::regs().addr_endp().write(|w| { + w.set_address(self.dev_addr); + w.set_endpoint(self.ep_addr); + }); + + self.ep_control().modify(|w| { + w.set_enable(true); + w.set_interrupt_per_buff(true); + w.set_buffer_address(self.buf.addr); + + let epty = match E::ep_type() { + EndpointType::Control => EpControlEndpointType::CONTROL, + EndpointType::Isochronous => EpControlEndpointType::ISOCHRONOUS, + EndpointType::Bulk => EpControlEndpointType::BULK, + EndpointType::Interrupt => EpControlEndpointType::INTERRUPT, + }; + + w.set_endpoint_type(epty); + }); + + regs.sie_ctrl().modify(|w| w.set_preamble_en(self.pre)); + } + } + + /// Clear current active channel and disable interrupt + /// + /// Safe to call outside of transfer context + fn clear_current(&self) { + // If this channel is selected + if self.is_ready_for_transaction() { + if !Self::is_interrupt_in() { + CURRENT_CHANNEL.store(0, Ordering::Relaxed); + } + + self.ep_control().modify(|w| { + w.set_interrupt_per_buff(false); + w.set_enable(false); + }); + + self.buffer_control().modify(|w| { + w.set_available(0, false); + }) + } + } + + /// Copy setup packet to buffer and set SETUP transaction + /// + /// Set PID = 1 for next transaction + fn set_setup_packet(&mut self, setup: &SetupPacket) { + assert!(E::ep_type() == EndpointType::Control); + let dpram = T::dpram(); + dpram.setup_packet_low().write(|w| { + w.set_bmrequesttype(setup.request_type.bits()); + w.set_brequest(setup.request); + w.set_wvalue(setup.value); + }); + dpram.setup_packet_high().write(|w| { + w.set_windex(setup.index); + w.set_wlength(setup.length); + }); + T::regs().sie_ctrl().modify(|w| { + w.set_send_data(false); + w.set_receive_data(false); + w.set_send_setup(true); + }); + + self.pid = true; + } + + /// Reload interrupt channel buffer register + fn interrupt_reload(&mut self) { + assert!(E::ep_type() == EndpointType::Interrupt); + let ctrl = self.buffer_control(); + ctrl.write(|w| { + w.set_last(0, true); + w.set_pid(0, self.pid); + w.set_full(0, false); + w.set_reset(true); + w.set_length(0, self.max_packet_size); + w.set_available(0, true); + }); + + self.pid = !self.pid; + // TODO: SOF? + // T::regs().sie_ctrl().modify(|w| { + // w.set_sof_en(true); + // w.set_keep_alive_en(true); + // w.set_pulldown_en(true); + // }); + + // FIXME: delay reason + cortex_m::asm::delay(12); + T::regs().int_ep_ctrl().modify(|w| { + w.set_int_ep_active(w.int_ep_active() | 1 << (self.index - 1)); + }); + } + + /// Set DATA IN transaction + /// + /// WARNING: This flips PID + fn set_data_in(&mut self, len: u16) { + assert!(E::ep_type() != EndpointType::Interrupt); + + self.buffer_control().write(|w| { + w.set_pid(0, self.pid); + w.set_full(0, false); + w.set_length(0, len); + w.set_last(0, true); + w.set_reset(true); + w.set_available(0, true); + }); + + self.pid = !self.pid; + + T::regs().sie_ctrl().modify(|w| { + w.set_send_data(false); + w.set_send_setup(false); + w.set_receive_data(true); + }); + } + + /// Set DATA OUT transaction and copy data to buffer + /// Returns count of copied bytes + fn set_data_out(&mut self, data: &[u8]) -> usize { + assert!(E::ep_type() != EndpointType::Interrupt); + + let chunk = if data.len() > 0 { + data.chunks(self.max_packet_size as _).next().unwrap() + } else { + &[] + }; + + self.buf.write(&chunk); + + self.buffer_control().write(|w| { + w.set_available(0, true); + w.set_pid(0, self.pid); + w.set_full(0, true); + w.set_length(0, chunk.len() as _); + w.set_last(0, true); + w.set_reset(true); + }); + + self.pid = !self.pid; + + T::regs().sie_ctrl().modify(|w| { + w.set_send_data(true); + w.set_send_setup(false); + w.set_receive_data(false); + }); + + chunk.len() + } + + /// Clear buffer interrupt bit + fn clear_sie_status(&self) { + if Self::is_interrupt_in() { + T::regs().buff_status().write_clear(|w| w.0 = 0b11 << self.index * 2); + } else { + T::regs().buff_status().write_clear(|w| w.0 = 0b11); + } + } + + /// Send SETUP packet + /// + /// WARNING: This flips PID + async fn send_setup(&mut self, setup: &SetupPacket) -> Result<(), ChannelError> { + // Wait transfer buffer to be free + self.wait_ready_for_transaction().await; + + // Set this channel for transaction + self.set_current(); + + trace!("SEND SETUP"); + // Prepare HW + self.set_setup_packet(setup); + + // Wait for SETUP end + let res = self.wait_transaction().await; + + self.clear_current(); + + res + } + + /// Send status packet + async fn control_status(&mut self, active_direction_out: bool) -> Result<(), ChannelError> { + // Wait transfer buffer to be free + self.wait_ready_for_transaction().await; + + // Set this channel for transaction + self.set_current(); + + // Status packet always have DATA1 + trace!("SEND STATUS"); + self.pid = true; + if active_direction_out { + self.set_data_in(0); + } else { + self.set_data_out(&[]); + } + + let res = self.wait_transaction().await; + + self.clear_current(); + + res + } +} + +impl<'d, T: Instance, E: channel::Type, D: channel::Direction> UsbChannel for Channel<'d, T, E, D> { + async fn control_in(&mut self, setup: &SetupPacket, buf: &mut [u8]) -> Result + where + E: channel::IsControl, + D: channel::IsIn, + { + trace!("CONTROL IN: {}", setup); + // Setup stage + // TODO: Whole transaction error handling? + self.send_setup(setup).await?; + + // Data stage + let read = if setup.length > 0 { + self.request_in(&mut buf[..setup.length as usize]).await? + } else { + 0 + }; + + // Status stage + self.control_status(false).await?; + + Ok(read) + } + + async fn control_out(&mut self, setup: &SetupPacket, buf: &[u8]) -> Result<(), ChannelError> + where + E: channel::IsControl, + D: channel::IsOut, + { + trace!("CONTROL OUT: {}", setup); + // Setup stage + // TODO: Whole transaction error handling? + self.send_setup(setup).await?; + + // Data stage + if setup.length > 0 { + self.request_out(&buf[..setup.length as usize]).await?; + } + + // Status stage + self.control_status(true).await?; + + Ok(()) + } + + fn retarget_channel(&mut self, addr: u8, endpoint: &EndpointInfo, pre: bool) -> Result<(), HostError> { + self.pre = pre; + self.dev_addr = addr; + self.max_packet_size = endpoint.max_packet_size; + Ok(()) + } + + async fn request_in(&mut self, buf: &mut [u8]) -> Result + where + D: channel::IsIn, + { + // Wait transfer buffer to be free + self.wait_ready_for_transaction().await; + + // Set this channel for transaction + self.set_current(); + + let mut count: usize = 0; + + let res = loop { + if Self::is_interrupt_in() { + trace!("CHANNEL {} WAIT FOR INTERRUPT", self.index); + self.interrupt_reload(); + self.wait_available().await; + } else { + trace!("CHANNEL {} START READ, len = {}", self.index, buf.len()); + self.set_data_in(buf[count..].len() as _); + if let Err(e) = self.wait_transaction().await { + break Err(e); + } + } + + let free = &mut buf[count..]; + let rx_len = self.buffer_control().read().length(0) as usize; + trace!("CHANNEL {} READ DONE, rx_len = {}", self.index, rx_len); + + if rx_len > free.len() { + break Err(ChannelError::BufferOverflow); + } + + self.buf.read(&mut free[..rx_len]); + count += rx_len; + + // If transfer is smaller than max_packet_size, we are done + // If we have read buf.len() bytes, we are done + if count == buf.len() || rx_len < self.max_packet_size as usize { + break Ok(count); + } + }; + + self.clear_current(); + + res + } + + async fn request_out(&mut self, buf: &[u8]) -> Result<(), ChannelError> + where + D: channel::IsOut, + { + // Wait transfer buffer to be free + self.wait_ready_for_transaction().await; + + let regs = T::regs(); + + // Set this channel for transaction + self.set_current(); + + let mut count = 0; + + let res = loop { + trace!("CHANNEL {} START WRITE", self.index); + let packet = self.set_data_out(buf); + + if let Err(e) = self.wait_transaction().await { + break Err(e); + } + + trace!("WRITE DONE, tx_len = {}", packet); + + count += packet; + + if count == buf.len() { + break Ok(()); + } + }; + + self.clear_current(); + res + } + + async fn set_timeout(&mut self, _: TimeoutConfig) { + todo!(); + } +} + +// TODO: channel should have reference to `allocated_pipes` +// impl<'d, T: Instance, E: channel::Type, D: channel::Direction> Drop for Channel<'d, T, E, D> { +// fn drop(&mut self) { +// if E::ep_type() == EndpointType::Interrupt { +// // Clear interrupts +// channel.clear_current(); +// self.allocated_pipes.fetch_and(!(1 << channel.index), Ordering::Relaxed); +// } +// } +// } + +impl<'d, T: Instance> UsbHostDriver for Driver<'d, T> { + type Channel = Channel<'d, T, E, D>; + + async fn wait_for_device_event(&self) -> DeviceEvent { + let is_connected = |status: u8| match status { + 0b01 | 0b10 => true, + _ => false, + }; + + // Read current state + let was = is_connected(T::regs().sie_status().read().speed()); + + // Clear interrupt status + T::regs().sie_status().write_clear(|w| { + w.set_speed(0b11); + }); + + // Enable conn/dis irq + T::regs().inte().modify(|w| { + w.set_host_conn_dis(true); + }); + let ev = poll_fn(|cx| { + BUS_WAKER.register(cx.waker()); + + let now = T::regs().sie_status().read().speed(); + let speed_now: DeviceEvent = match now { + 0b01 => DeviceEvent::Connected(Speed::Low), + 0b10 => DeviceEvent::Connected(Speed::Full), + _ => DeviceEvent::Disconnected, + }; + match (was, is_connected(now)) { + (true, false) => Poll::Ready(DeviceEvent::Disconnected), + (false, true) => Poll::Ready(speed_now), + _ => Poll::Pending, + } + }) + .await; + ev + } + + async fn bus_reset(&self) { + T::regs().sie_ctrl().modify(|w| { + w.set_reset_bus(true); + }); + + embassy_time::Timer::after_millis(50).await; + } + + fn alloc_channel( + &self, + dev_addr: u8, + endpoint: &EndpointInfo, + pre: bool, + ) -> Result, HostError> { + if E::ep_type() == EndpointType::Interrupt { + let alloc = self.allocated_pipes.load(Ordering::Acquire); + let free_index = (1..16) + .find(|i| alloc & (1 << i) == 0) + .ok_or(HostError::OutOfChannels)? as u8; + + self.allocated_pipes.store(alloc | 1 << free_index, Ordering::Release); + // Use fixed layout + let addr = DPRAM_DATA_OFFSET + MAIN_BUFFER_SIZE as u16 + free_index as u16 * 64; + + Ok(Channel::new(free_index as _, addr, 64, endpoint, dev_addr, pre)) + } else { + let index = self.channel_index.fetch_add(1, Ordering::Relaxed); + Ok(Channel::new( + index, + DPRAM_DATA_OFFSET, + MAIN_BUFFER_SIZE as u16, + endpoint, + dev_addr, + pre, + )) + } + } +} + +/// USB interrupt handler. +pub struct InterruptHandler { + _usb: PhantomData, +} + +impl interrupt::typelevel::Handler for InterruptHandler { + unsafe fn on_interrupt() { + let regs = T::regs(); + let ints = regs.ints().read(); + + let ev = { + if ints.host_conn_dis() { + regs.inte().write_clear(|w| w.set_host_conn_dis(true)); + match regs.sie_status().read().speed() { + 0b01 => "attached low speed", + 0b10 => "attached full speed", + _ => "detached", + } + } else if ints.host_resume() { + regs.sie_status().write_clear(|w| w.set_resume(true)); + "resume" + } else if ints.error_crc() { + regs.sie_status().write_clear(|w| w.set_crc_error(true)); + "crc error" + } else if ints.error_bit_stuff() { + regs.sie_status().write_clear(|w| w.set_bit_stuff_error(true)); + "bit stuff error" + } else if ints.error_data_seq() { + regs.sie_status().write_clear(|w| w.set_data_seq_error(true)); + "data sequence error" + } else if ints.stall() { + regs.inte().write_clear(|w| w.set_stall(true)); + EP_IN_WAKERS[0].wake(); + "stall" + } else if ints.error_rx_overflow() { + regs.inte().write_clear(|w| w.set_error_rx_overflow(true)); + EP_IN_WAKERS[0].wake(); + "rx overflow" + } else if ints.trans_complete() { + regs.inte().write_clear(|w| w.set_trans_complete(true)); + EP_IN_WAKERS[0].wake(); + "transaction complete" + } else if ints.error_rx_timeout() { + regs.inte().write_clear(|w| w.set_error_rx_timeout(true)); + EP_IN_WAKERS[0].wake(); + "rx timeout" + } else if ints.buff_status() { + let status = regs.buff_status().read().0; + for i in 0..32 { + // ith bit set + if (status >> i) & 1 == 1 { + regs.buff_status().write_clear(|w| w.0 = 1 << i); + // control transfers (buffer 0) + if i != 0 { + let idx = i / 2; + // T::regs().int_ep_ctrl().modify(|w| { + // w.set_int_ep_active(w.int_ep_active() | 1 << idx); + // }); + trace!("USB IRQ: Interrupt EP {}", idx); + EP_IN_WAKERS[idx].wake(); + } else { + trace!("USB IRQ: EPx"); + EP_IN_WAKERS[0].wake(); + } + break; + } + } + "^^^" + } else if ints.host_sof() { + // Prevent nonstop SOF interrupt + T::regs().inte().write_clear(|w| w.set_host_sof(true)); + "sof" + } else { + "???" + } + }; + + trace!("USB IRQ: {:08x} :: {}", ints.0, ev); + + BUS_WAKER.wake(); + } +} diff --git a/embassy-rp/src/usb/mod.rs b/embassy-rp/src/usb/mod.rs new file mode 100644 index 0000000000..9ed6a9a0fb --- /dev/null +++ b/embassy-rp/src/usb/mod.rs @@ -0,0 +1,112 @@ +//! USB driver. + +use core::marker::PhantomData; +use core::slice; + +use atomic_polyfill::{compiler_fence, Ordering}; +use embassy_sync::waitqueue::AtomicWaker; +use embassy_usb_driver::Direction; + +use crate::{interrupt, pac, peripherals}; + +trait SealedInstance { + fn regs() -> crate::pac::usb::Usb; + fn dpram() -> crate::pac::usb_dpram::UsbDpram; + + // FIXME(svd): Add EPX to svd + fn dpram_epx_control() -> pac::common::Reg; +} + +/// USB peripheral instance. +#[allow(private_bounds)] +pub trait Instance: SealedInstance + 'static { + /// Interrupt for this peripheral. + type Interrupt: interrupt::typelevel::Interrupt; +} + +impl crate::usb::SealedInstance for peripherals::USB { + fn regs() -> pac::usb::Usb { + pac::USB + } + fn dpram() -> pac::usb_dpram::UsbDpram { + pac::USB_DPRAM + } + + fn dpram_epx_control() -> pac::common::Reg { + unsafe { pac::common::Reg::from_ptr((Self::dpram().as_ptr().byte_offset(0x100)) as _) } + } +} + +impl crate::usb::Instance for peripherals::USB { + type Interrupt = crate::interrupt::typelevel::USBCTRL_IRQ; +} + +const EP_COUNT: usize = 16; +const EP_MEMORY_SIZE: usize = 4096; +const EP_MEMORY: *mut u8 = pac::USB_DPRAM.as_ptr() as *mut u8; +const DPRAM_DATA_OFFSET: u16 = 0x180; + +const NEW_AW: AtomicWaker = AtomicWaker::new(); +static BUS_WAKER: AtomicWaker = NEW_AW; +static EP_IN_WAKERS: [AtomicWaker; EP_COUNT] = [NEW_AW; EP_COUNT]; +static EP_OUT_WAKERS: [AtomicWaker; EP_COUNT] = [NEW_AW; EP_COUNT]; + +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +struct EndpointBuffer { + addr: u16, + len: u16, + _phantom: PhantomData, +} + +impl EndpointBuffer { + const fn new(addr: u16, len: u16) -> Self { + Self { + addr, + len, + _phantom: PhantomData, + } + } + + fn read(&mut self, buf: &mut [u8]) { + assert!(buf.len() <= self.len as usize); + compiler_fence(Ordering::SeqCst); + let mem = unsafe { slice::from_raw_parts(EP_MEMORY.add(self.addr as _), buf.len()) }; + buf.copy_from_slice(mem); + compiler_fence(Ordering::SeqCst); + } + + fn write(&mut self, buf: &[u8]) { + assert!(buf.len() <= self.len as usize); + compiler_fence(Ordering::SeqCst); + let mem = unsafe { slice::from_raw_parts_mut(EP_MEMORY.add(self.addr as _), buf.len()) }; + mem.copy_from_slice(buf); + compiler_fence(Ordering::SeqCst); + } +} + +trait Dir { + fn dir() -> Direction; +} + +/// Type for In direction. +pub enum In {} +impl Dir for In { + fn dir() -> Direction { + Direction::In + } +} + +/// Type for Out direction. +pub enum Out {} +impl Dir for Out { + fn dir() -> Direction { + Direction::Out + } +} + +pub mod device; +pub mod host; + +// FIXME: Compat +pub use device::*; diff --git a/embassy-stm32/src/usb/usb.rs b/embassy-stm32/src/usb/usb.rs index 3e8e74a1ff..917e8db639 100644 --- a/embassy-stm32/src/usb/usb.rs +++ b/embassy-stm32/src/usb/usb.rs @@ -1,5 +1,8 @@ #![macro_use] +#[cfg(usb_v4)] +mod usb_host; + use core::future::poll_fn; use core::marker::PhantomData; use core::sync::atomic::{AtomicBool, Ordering}; @@ -11,6 +14,8 @@ use embassy_usb_driver as driver; use embassy_usb_driver::{ Direction, EndpointAddress, EndpointAllocError, EndpointError, EndpointInfo, EndpointType, Event, Unsupported, }; +#[cfg(usb_v4)] +pub use usb_host::*; use crate::pac::usb::regs; use crate::pac::usb::vals::{EpType, Stat}; diff --git a/embassy-stm32/src/usb/usb_host.rs b/embassy-stm32/src/usb/usb_host.rs new file mode 100644 index 0000000000..e65ae7935b --- /dev/null +++ b/embassy-stm32/src/usb/usb_host.rs @@ -0,0 +1,768 @@ +#![macro_use] +#![allow(missing_docs)] +use core::future::poll_fn; +use core::marker::PhantomData; +use core::sync::atomic::{AtomicU16, AtomicU32, Ordering}; +use core::task::Poll; + +use embassy_hal_internal::PeripheralType; +use embassy_sync::waitqueue::AtomicWaker; +use embassy_time::{Duration, Instant, Timer}; +use embassy_usb_driver::host::{ + channel, ChannelError, DeviceEvent, HostError, TimeoutConfig, UsbChannel, UsbHostDriver, +}; +use embassy_usb_driver::{EndpointType, Speed}; +use stm32_metapac::common::{Reg, RW}; +use stm32_metapac::usb::regs::Epr; + +use super::{DmPin, DpPin, Instance}; +use crate::interrupt; +use crate::pac::usb::regs; +use crate::pac::usb::vals::{EpType, Stat}; +use crate::pac::USBRAM; + +/// The number of registers is 8, allowing up to 16 mono- +/// directional/single-buffer or up to 7 double-buffer endpoints in any combination. For +/// example the USB peripheral can be programmed to have 4 double buffer endpoints +/// and 8 single-buffer/mono-directional endpoints. +const USB_MAX_PIPES: usize = 8; + +/// Interrupt handler. +pub struct USBHostInterruptHandler { + _phantom: PhantomData, +} + +impl interrupt::typelevel::Handler for USBHostInterruptHandler { + unsafe fn on_interrupt() { + let regs = I::regs(); + // let x = regs.istr().read().0; + // trace!("USB IRQ: {:08x}", x); + + let istr = regs.istr().read(); + + // Detect device connect/disconnect + if istr.reset() { + trace!("USB IRQ: device connect/disconnect"); + + // Write 0 to clear. + let mut clear = regs::Istr(!0); + clear.set_reset(false); + regs.istr().write_value(clear); + + // Wake main thread. + BUS_WAKER.wake(); + } + + if istr.ctr() { + let index = istr.ep_id() as usize; + + let epr = regs.epr(index).read(); + + let mut epr_value = invariant(epr); + // Check and clear error flags + if epr.err_tx() { + epr_value.set_err_tx(false); + warn!("err_tx"); + } + if epr.err_rx() { + epr_value.set_err_rx(false); + warn!("err_rx"); + } + // Clear ctr (transaction complete) flags + let rx_ready = epr.ctr_rx(); + let tx_ready = epr.ctr_tx(); + + epr_value.set_ctr_rx(!rx_ready); + epr_value.set_ctr_tx(!tx_ready); + regs.epr(index).write_value(epr_value); + + if rx_ready { + EP_IN_WAKERS[index].wake(); + } + if tx_ready { + EP_OUT_WAKERS[index].wake(); + } + } + + if istr.err() { + debug!("USB IRQ: err"); + regs.istr().write_value(regs::Istr(!0)); + + // Write 0 to clear. + let mut clear = regs::Istr(!0); + clear.set_err(false); + regs.istr().write_value(clear); + + let index = istr.ep_id() as usize; + let mut epr = invariant(regs.epr(index).read()); + // Toggle endpoint to disabled + epr.set_stat_rx(epr.stat_rx()); + epr.set_stat_tx(epr.stat_tx()); + regs.epr(index).write_value(epr); + } + } +} + +const EP_COUNT: usize = 8; + +#[cfg(any(usbram_16x1_512, usbram_16x2_512))] +const USBRAM_SIZE: usize = 512; +#[cfg(any(usbram_16x2_1024, usbram_32_1024))] +const USBRAM_SIZE: usize = 1024; +#[cfg(usbram_32_2048)] +const USBRAM_SIZE: usize = 2048; + +#[cfg(not(any(usbram_32_2048, usbram_32_1024)))] +const USBRAM_ALIGN: usize = 2; +#[cfg(any(usbram_32_2048, usbram_32_1024))] +const USBRAM_ALIGN: usize = 4; + +const NEW_AW: AtomicWaker = AtomicWaker::new(); +static BUS_WAKER: AtomicWaker = NEW_AW; +static EP_IN_WAKERS: [AtomicWaker; EP_COUNT] = [NEW_AW; EP_COUNT]; +static EP_OUT_WAKERS: [AtomicWaker; EP_COUNT] = [NEW_AW; EP_COUNT]; + +fn convert_type(t: EndpointType) -> EpType { + match t { + EndpointType::Bulk => EpType::BULK, + EndpointType::Control => EpType::CONTROL, + EndpointType::Interrupt => EpType::INTERRUPT, + EndpointType::Isochronous => EpType::ISO, + } +} + +fn invariant(mut r: regs::Epr) -> regs::Epr { + r.set_ctr_rx(true); // don't clear + r.set_ctr_tx(true); // don't clear + r.set_dtog_rx(false); // don't toggle + r.set_dtog_tx(false); // don't toggle + r.set_stat_rx(Stat::from_bits(0)); + r.set_stat_tx(Stat::from_bits(0)); + r +} + +fn align_len_up(len: u16) -> u16 { + ((len as usize + USBRAM_ALIGN - 1) / USBRAM_ALIGN * USBRAM_ALIGN) as u16 +} + +/// Calculates the register field values for configuring receive buffer descriptor. +/// Returns `(actual_len, len_bits)` +/// +/// `actual_len` length in bytes rounded up to USBRAM_ALIGN +/// `len_bits` should be placed on the upper 16 bits of the register value +fn calc_receive_len_bits(len: u16) -> (u16, u16) { + match len { + // NOTE: this could be 2..=62 with 16bit USBRAM, but not with 32bit. Limit it to 60 for simplicity. + 2..=60 => (align_len_up(len), align_len_up(len) / 2 << 10), + 61..=1024 => ((len + 31) / 32 * 32, (((len + 31) / 32 - 1) << 10) | 0x8000), + _ => panic!("invalid OUT length {}", len), + } +} + +#[cfg(any(usbram_32_2048, usbram_32_1024))] +mod btable { + use super::*; + + pub(super) fn write_in(_index: usize, _addr: u16) {} + + /// Writes to Transmit Buffer Descriptor for Channel/endpoint `index`` + /// For Device this is an IN endpoint for Host an OUT endpoint + pub(super) fn write_transmit_buffer_descriptor(index: usize, addr: u16, len: u16) { + // Address offset: index*8 [bytes] thus index*2 in 32 bit words + USBRAM.mem(index * 2).write_value((addr as u32) | ((len as u32) << 16)); + } + + /// Writes to Receive Buffer Descriptor for Channel/endpoint `index`` + /// For Device this is an OUT endpoint for Host an IN endpoint + pub(super) fn write_receive_buffer_descriptor(index: usize, addr: u16, max_len_bits: u16) { + // Address offset: index*8 + 4 [bytes] thus index*2 + 1 in 32 bit words + USBRAM + .mem(index * 2 + 1) + .write_value((addr as u32) | ((max_len_bits as u32) << 16)); + } + + pub(super) fn read_out_len(index: usize) -> u16 { + (USBRAM.mem(index * 2 + 1).read() >> 16) as u16 + } +} + +// Maybe replace with struct that only knows its index +struct EndpointBuffer { + addr: u16, + len: u16, + _phantom: PhantomData, +} + +impl EndpointBuffer { + fn new(addr: u16, len: u16) -> Self { + EndpointBuffer { + addr, + len, + _phantom: PhantomData, + } + } + + fn read(&mut self, buf: &mut [u8]) { + assert!(buf.len() <= self.len as usize); + for i in 0..(buf.len() + USBRAM_ALIGN - 1) / USBRAM_ALIGN { + let val = USBRAM.mem(self.addr as usize / USBRAM_ALIGN + i).read(); + let n = USBRAM_ALIGN.min(buf.len() - i * USBRAM_ALIGN); + buf[i * USBRAM_ALIGN..][..n].copy_from_slice(&val.to_le_bytes()[..n]); + } + } + + fn write(&mut self, buf: &[u8]) { + assert!(buf.len() <= self.len as usize); + for i in 0..(buf.len() + USBRAM_ALIGN - 1) / USBRAM_ALIGN { + let mut val = [0u8; USBRAM_ALIGN]; + let n = USBRAM_ALIGN.min(buf.len() - i * USBRAM_ALIGN); + val[..n].copy_from_slice(&buf[i * USBRAM_ALIGN..][..n]); + + #[cfg(not(any(usbram_32_2048, usbram_32_1024)))] + let val = u16::from_le_bytes(val); + #[cfg(any(usbram_32_2048, usbram_32_1024))] + let val = u32::from_le_bytes(val); + USBRAM.mem(self.addr as usize / USBRAM_ALIGN + i).write_value(val); + } + } +} + +/// First bit is used to indicate control pipes +/// bitfield for keeping track of used channels +static ALLOCATED_PIPES: AtomicU32 = AtomicU32::new(0); +static EP_MEM_FREE: AtomicU16 = AtomicU16::new(0); + +/// USB host driver. +pub struct UsbHost<'d, I: Instance> { + phantom: PhantomData<&'d mut I>, + // first free address in EP mem, in bytes. + // ep_mem_free: u16, +} + +impl<'d, I: Instance> UsbHost<'d, I> { + /// Create a new USB driver. + pub fn new( + _usb: impl PeripheralType + 'd, + _irq: impl interrupt::typelevel::Binding> + 'd, + dp: impl PeripheralType + DpPin + 'd, + dm: impl PeripheralType + DmPin + 'd, + ) -> Self { + super::super::common_init::(); + + let regs = I::regs(); + + regs.cntr().write(|w| { + w.set_pdwn(false); + w.set_fres(true); + w.set_host(true); + }); + + // Wait for voltage reference + #[cfg(feature = "time")] + embassy_time::block_for(embassy_time::Duration::from_millis(100)); + #[cfg(not(feature = "time"))] + cortex_m::asm::delay(unsafe { crate::rcc::get_freqs() }.sys.unwrap().0 / 10); + + #[cfg(not(usb_v4))] + regs.btable().write(|w| w.set_btable(0)); + + #[cfg(not(stm32l1))] + { + use crate::gpio::{AfType, OutputType, Speed}; + dp.set_as_af(dp.af_num(), AfType::output(OutputType::PushPull, Speed::VeryHigh)); + dm.set_as_af(dm.af_num(), AfType::output(OutputType::PushPull, Speed::VeryHigh)); + } + #[cfg(stm32l1)] + let _ = (dp, dm); // suppress "unused" warnings. + + EP_MEM_FREE.store(EP_COUNT as u16 * 8, Ordering::Relaxed); + Self { + phantom: PhantomData, + // ep_mem_free: EP_COUNT as u16 * 8, // for each EP, 4 regs, so 8 bytes + // control_channel_in: Channel::new(0, 0, 0, 0), + // control_channel_out: Channel::new(0, 0, 0, 0), + // channels_used: 0, + // channels_out_used: 0, + } + } + + /// Start the USB peripheral + pub fn start(&mut self) { + let regs = I::regs(); + + regs.cntr().write(|w| { + w.set_host(true); + w.set_pdwn(false); + w.set_fres(false); + // Masks + w.set_resetm(true); + w.set_suspm(false); + w.set_wkupm(false); + w.set_ctrm(true); + w.set_errm(false); + }); + + // Enable pull downs on DP and DM lines for host mode + #[cfg(any(usb_v3, usb_v4))] + regs.bcdr().write(|w| w.set_dppu(true)); + + #[cfg(stm32l1)] + crate::pac::SYSCFG.pmc().modify(|w| w.set_usb_pu(true)); + } + + pub fn get_status(&self) -> u32 { + let regs = I::regs(); + + let istr = regs.istr().read(); + + istr.0 + } + + fn alloc_channel_mem(&self, len: u16) -> Result { + assert!(len as usize % USBRAM_ALIGN == 0); + let addr = EP_MEM_FREE.load(Ordering::Relaxed); + if addr + len > USBRAM_SIZE as _ { + // panic!("Endpoint memory full"); + error!("Endpoint memory full"); + return Err(()); + } + EP_MEM_FREE.store(addr + len, Ordering::Relaxed); + Ok(addr) + } + + async fn wait_for_device_connect(&self) -> DeviceEvent { + poll_fn(|cx| { + let istr = I::regs().istr().read(); + + BUS_WAKER.register(cx.waker()); + + if istr.dcon_stat() { + let speed = if istr.ls_dcon() { Speed::Low } else { Speed::Full }; + // device has been detected + Poll::Ready(DeviceEvent::Connected(speed)) + } else { + Poll::Pending + } + }) + .await + } + + async fn wait_for_device_disconnect(&self) -> DeviceEvent { + poll_fn(|cx| { + let istr = I::regs().istr().read(); + + BUS_WAKER.register(cx.waker()); + + if !istr.dcon_stat() { + // device has disconnected + Poll::Ready(DeviceEvent::Disconnected) + } else { + Poll::Pending + } + }) + .await + } +} + +/// USB endpoint. Only implements single buffer mode. +pub struct Channel<'d, I: Instance, D: channel::Direction, T: channel::Type> { + _phantom: PhantomData<(&'d mut I, D, T)>, + /// Register index (there are 8 in total) + index: usize, + max_packet_size_in: u16, + max_packet_size_out: u16, + buf_in: Option>, + buf_out: Option>, +} + +impl<'d, I: Instance, D: channel::Direction, T: channel::Type> Channel<'d, I, D, T> { + fn new( + index: usize, + buf_in: Option>, + buf_out: Option>, + max_packet_size_in: u16, + max_packet_size_out: u16, + ) -> Self { + Self { + _phantom: PhantomData, + index, + max_packet_size_in, + max_packet_size_out, + buf_in, + buf_out, + } + } + + fn reg(&self) -> Reg { + I::regs().epr(self.index) + } + + pub fn activate_rx(&mut self) { + let epr = self.reg(); + let epr_val = epr.read(); + let current_stat_rx = epr_val.stat_rx().to_bits(); + let mut epr_val = invariant(epr_val); + // stat_rx can only be toggled by writing a 1. + // We want to set it to Valid (0b11) + let stat_mask = Stat::from_bits(!current_stat_rx & 0x3); + epr_val.set_stat_rx(stat_mask); + epr.write_value(epr_val); + } + + pub fn activate_tx(&mut self) { + let epr = self.reg(); + let epr_val = epr.read(); + let current_stat_tx = epr_val.stat_tx().to_bits(); + let mut epr_val = invariant(epr_val); + // stat_tx can only be toggled by writing a 1. + // We want to set it to Valid (0b11) + let stat_mask = Stat::from_bits(!current_stat_tx & 0x3); + epr_val.set_stat_tx(stat_mask); + epr.write_value(epr_val); + } + + pub fn disable_rx(&mut self) { + let epr = self.reg(); + let epr_val = epr.read(); + let current_stat_rx = epr_val.stat_rx(); + let mut epr_val = invariant(epr_val); + // stat_rx can only be toggled by writing a 1. + // We want to set it to Disabled (0b00). + epr_val.set_stat_rx(current_stat_rx); + epr.write_value(epr_val); + } + + fn disable_tx(&mut self) { + let epr = self.reg(); + let epr_val = epr.read(); + let current_stat_tx = epr_val.stat_tx(); + let mut epr_val = invariant(epr_val); + // stat_tx can only be toggled by writing a 1. + // We want to set it to InActive (0b00). + epr_val.set_stat_tx(current_stat_tx); + epr.write_value(epr_val); + } + + fn read_data(&mut self, buf: &mut [u8]) -> Result { + let index = self.index; + let rx_len = btable::read_out_len::(index) as usize & 0x3FF; + trace!("READ DONE, rx_len = {}", rx_len); + if rx_len > buf.len() { + return Err(ChannelError::BufferOverflow); + } + self.buf_in.as_mut().unwrap().read(&mut buf[..rx_len]); + Ok(rx_len) + } + + fn write_data(&mut self, buf: &[u8]) { + let index = self.index; + if let Some(buf_out) = self.buf_out.as_mut() { + buf_out.write(buf); + btable::write_transmit_buffer_descriptor::(index, buf_out.addr, buf.len() as _); + } + } + + async fn write(&mut self, buf: &[u8]) -> Result<(), ChannelError> { + self.write_data(buf); + + let index = self.index; + let timeout_ms = 1000; + + self.activate_tx(); + + let regs = I::regs(); + + let t0 = Instant::now(); + + poll_fn(|cx| { + EP_OUT_WAKERS[index].register(cx.waker()); + + // Detect disconnect + let istr = regs.istr().read(); + if !istr.dcon_stat() { + self.disable_tx(); + return Poll::Ready(Err(ChannelError::Disconnected)); + } + + if t0.elapsed() > Duration::from_millis(timeout_ms as u64) { + // Timeout, we need to stop the current transaction. + self.disable_tx(); + return Poll::Ready(Err(ChannelError::Timeout)); + } + + let stat = self.reg().read().stat_tx(); + match stat { + Stat::DISABLED => Poll::Ready(Ok(())), + Stat::STALL => Poll::Ready(Err(ChannelError::Stall)), + Stat::NAK | Stat::VALID => Poll::Pending, + } + }) + .await + } + + async fn read(&mut self, buf: &mut [u8]) -> Result { + let index = self.index; + + let timeout_ms = 1000; + + self.activate_rx(); + + let regs = I::regs(); + + let mut count: usize = 0; + + let t0 = Instant::now(); + + poll_fn(|cx| { + EP_IN_WAKERS[index].register(cx.waker()); + + // Detect disconnect + let istr = regs.istr().read(); + if !istr.dcon_stat() { + self.disable_rx(); + return Poll::Ready(Err(ChannelError::Disconnected)); + } + + if t0.elapsed() > Duration::from_millis(timeout_ms as u64) { + self.disable_rx(); + return Poll::Ready(Err(ChannelError::Timeout)); + } + + let stat = self.reg().read().stat_rx(); + match stat { + Stat::DISABLED => { + // Data available for read + let idest = &mut buf[count..]; + let n = self.read_data(idest)?; + count += n; + // If transfer is smaller than max_packet_size, we are done + // If we have read buf.len() bytes, we are done + if count == buf.len() || n < self.max_packet_size_in as usize { + Poll::Ready(Ok(count)) + } else { + // More data expected: issue another read. + self.activate_rx(); + Poll::Pending + } + } + Stat::STALL => { + // error + Poll::Ready(Err(ChannelError::Stall)) + } + Stat::NAK => Poll::Pending, + Stat::VALID => { + // not started yet? Try again + Poll::Pending + } + } + }) + .await + } +} + +impl<'d, I: Instance, T: channel::Type, D: channel::Direction> UsbChannel for Channel<'d, I, D, T> { + async fn control_in( + &mut self, + setup: &embassy_usb_driver::host::SetupPacket, + buf: &mut [u8], + ) -> Result + where + T: channel::IsControl, + D: channel::IsIn, + { + let epr0 = I::regs().epr(0); + + // setup stage + let mut epr_val = invariant(epr0.read()); + epr_val.set_setup(true); + epr0.write_value(epr_val); + + self.write(setup.as_bytes()).await?; + + // data stage + let count = self.read(buf).await?; + + // status stage + + // Send 0 bytes + let zero: [u8; 0] = [0u8; 0]; + self.write(&zero).await?; + + Ok(count) + } + + async fn control_out( + &mut self, + setup: &embassy_usb_driver::host::SetupPacket, + buf: &[u8], + ) -> Result<(), ChannelError> + where + T: channel::IsControl, + D: channel::IsOut, + { + let epr0 = I::regs().epr(0); + + // setup stage + let mut epr_val = invariant(epr0.read()); + epr_val.set_setup(true); + epr0.write_value(epr_val); + self.write(setup.as_bytes()).await?; + + if buf.is_empty() { + // do nothing + } else { + self.write(buf).await?; + } + + // Status stage + let mut status = [0u8; 0]; + self.read(&mut status).await?; + + Ok(()) + } + + fn retarget_channel( + &mut self, + addr: u8, + endpoint: &embassy_usb_driver::EndpointInfo, + pre: bool, + ) -> Result<(), embassy_usb_driver::host::HostError> { + trace!( + "retarget_channel: addr: {:?} ep_type: {:?} index: {}", + addr, + endpoint.ep_type, + self.index + ); + let eptype = endpoint.ep_type; + let index = self.index; + + // configure channel register + let epr_reg = I::regs().epr(index); + let mut epr = invariant(epr_reg.read()); + epr.set_devaddr(addr); + epr.set_ep_type(convert_type(eptype)); + epr.set_ea(index as _); + epr_reg.write_value(epr); + + Ok(()) + } + + async fn request_in(&mut self, buf: &mut [u8]) -> Result + where + D: channel::IsIn, + { + self.read(buf).await + } + + async fn request_out(&mut self, buf: &[u8]) -> Result<(), ChannelError> + where + D: channel::IsOut, + { + self.write(buf).await + } + + async fn set_timeout(&mut self, _: TimeoutConfig) {} +} + +impl<'d, I: Instance, T: channel::Type, D: channel::Direction> Drop for Channel<'d, I, D, T> { + fn drop(&mut self) { + critical_section::with(|_| { + ALLOCATED_PIPES.store( + ALLOCATED_PIPES.load(Ordering::Relaxed) & !(1 << self.index), + Ordering::Relaxed, + ); + }); + } +} + +impl<'d, I: Instance> UsbHostDriver for UsbHost<'d, I> { + type Channel = Channel<'d, I, D, T>; + + fn alloc_channel( + &self, + addr: u8, + endpoint: &embassy_usb_driver::EndpointInfo, + pre: bool, + ) -> Result, embassy_usb_driver::host::HostError> { + let new_index = if T::ep_type() == EndpointType::Control { + // Only a single control channel is available + 0 + } else { + critical_section::with(|_| { + let pipes = ALLOCATED_PIPES.load(Ordering::Relaxed); + + // Ignore index 0 + let new_index = (pipes | 1).trailing_ones(); + if new_index as usize >= USB_MAX_PIPES { + Err(HostError::OutOfChannels) + } else { + ALLOCATED_PIPES.store(pipes | 1 << new_index, Ordering::Relaxed); + Ok(new_index) + } + })? + }; + + let max_packet_size = endpoint.max_packet_size; + + let buffer_in = if D::is_in() { + let (len, len_bits) = calc_receive_len_bits(max_packet_size); + let Ok(buffer_addr) = self.alloc_channel_mem(len) else { + return Err(HostError::OutOfSlots); + }; + + btable::write_receive_buffer_descriptor::(new_index as usize, buffer_addr, len_bits); + + Some(EndpointBuffer::new(buffer_addr, len)) + } else { + None + }; + + let buffer_out = if D::is_out() { + let len = align_len_up(max_packet_size); + let Ok(buffer_addr) = self.alloc_channel_mem(len) else { + return Err(HostError::OutOfSlots); + }; + + // ep_in_len is written when actually TXing packets. + btable::write_in::(new_index as usize, buffer_addr); + + Some(EndpointBuffer::new(buffer_addr, len)) + } else { + None + }; + + let mut channel = Channel::::new( + new_index as usize, + buffer_in, + buffer_out, + endpoint.max_packet_size, + endpoint.max_packet_size, + ); + + channel.retarget_channel(addr, endpoint, pre)?; + Ok(channel) + } + + async fn bus_reset(&self) { + let regs = I::regs(); + + trace!("Bus reset"); + // Set bus in reset state + regs.cntr().modify(|w| { + w.set_fres(true); + }); + + // USB Spec says wait 50ms + Timer::after_millis(50).await; + + // Clear reset state; device will be in default state + regs.cntr().modify(|w| { + w.set_fres(false); + }); + } + + async fn wait_for_device_event(&self) -> embassy_usb_driver::host::DeviceEvent { + // TODO: which event do we expect? + self.wait_for_device_connect().await + } +} diff --git a/embassy-usb-driver/Cargo.toml b/embassy-usb-driver/Cargo.toml index e40421649d..838a3c347d 100644 --- a/embassy-usb-driver/Cargo.toml +++ b/embassy-usb-driver/Cargo.toml @@ -21,3 +21,4 @@ features = ["defmt"] [dependencies] embedded-io-async = "0.6.1" defmt = { version = "1", optional = true } +bitflags = "2.6.0" diff --git a/embassy-usb-driver/src/host.rs b/embassy-usb-driver/src/host.rs new file mode 100644 index 0000000000..90f59d3c64 --- /dev/null +++ b/embassy-usb-driver/src/host.rs @@ -0,0 +1,301 @@ +//! USB host driver traits and data types. + +use core::time::Duration; + +use crate::{EndpointInfo, EndpointType, Speed}; + +/// Errors returned by [`ChannelOut::write`] and [`ChannelIn::read`] +#[derive(Copy, Clone, Eq, PartialEq, Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum ChannelError { + /// The packet is too long to fit in the buffer. + BufferOverflow, + + /// Response from device/bus was not interpretable (Crc, Babble) + BadResponse, + + /// Transaction was canceled + Canceled, + + /// The device endpoint is stalled. + Stall, + + /// Device did not respond in time + Timeout, + + /// Device disconnected + Disconnected, +} + +macro_rules! bitflags { + ($($tt:tt)*) => { + #[cfg(feature = "defmt")] + defmt::bitflags! { $($tt)* } + #[cfg(not(feature = "defmt"))] + bitflags::bitflags! { $($tt)* } + }; +} + +bitflags! { + #[cfg_attr(not(feature = "defmt"), derive(Copy, Clone, Eq, PartialEq, Debug))] + /// RequestType bitfields for the setup packet + pub struct RequestType: u8 { + // Recipient + /// The request is intended for the entire device. + const RECIPIENT_DEVICE = 0; + /// The request is intended for an interface. + const RECIPIENT_INTERFACE = 1; + /// The request is intended for an endpoint. + const RECIPIENT_ENDPOINT = 2; + /// The recipient of the request is unspecified. + const RECIPIENT_OTHER = 3; + + // Type + /// The request is a standard USB request. + const TYPE_STANDARD = 0 << 5; + /// The request is a class-specific request. + const TYPE_CLASS = 1 << 5; + /// The request is a vendor-specific request. + const TYPE_VENDOR = 2 << 5; + /// Reserved. + const TYPE_RESERVED = 3 << 5; + + // Direction + /// The request will send data to the device. + const OUT = 0 << 7; + /// The request expects to receive data from the device. + const IN = 1 << 7; + } +} + +/// USB Control Setup Packet +#[repr(C)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[derive(Copy, Clone, Eq, PartialEq, Debug)] +pub struct SetupPacket { + /// Request characteristics: direction, type, recipient. + /// See RequestType type for details. + /// Called bmRequestType in USB spec (Table 9-2). + pub request_type: RequestType, + /// Request code. + /// See Table 9-3 of USB spec for standard ones. + /// Called bRequest in USB spec (Table 9-2). + pub request: u8, + /// Use depending on request field. + /// Called wValue in USB spec (Table 9-2). + pub value: u16, + /// Use depending on request field. + /// Called wIndex in USB spec (Table 9-2). + pub index: u16, + /// Number of bytes to transfer in data stage if there is one. + /// Called wLength in USB spec (Table 9-2). + pub length: u16, +} + +impl SetupPacket { + /// Get a reference to the underlying bytes of the setup packet. + pub fn as_bytes(&self) -> &[u8] { + // Safe because we know that the size of SetupPacket is 8 bytes. + unsafe { core::slice::from_raw_parts(self as *const _ as *const u8, core::mem::size_of::()) } + } +} + +/// Device has been attached/detached +#[derive(Copy, Clone, Eq, PartialEq, Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum DeviceEvent { + /// Indicates a root-device has become attached + Connected(Speed), + /// Indicates that a device has been detached + Disconnected, +} + +/// Indicates type of error of Host interface +#[derive(Copy, Clone, Eq, PartialEq, Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum HostError { + ChannelError(ChannelError), + RequestFailed, + InvalidDescriptor, + OutOfSlots, + OutOfChannels, + NoSuchDevice, + InsufficientMemory, + Other(&'static str), +} + +impl From for HostError { + fn from(value: ChannelError) -> Self { + HostError::ChannelError(value) + } +} + +/// Async USB Host Driver trait. +/// To be implemented by the HAL. +pub trait UsbHostDriver: Sized { + /// Channel implementation of this UsbHostDriver + type Channel: UsbChannel; + + /// Wait for device connect or disconnect + /// + /// When connected, this function must issue a bus reset before the speed is reported + async fn wait_for_device_event(&self) -> DeviceEvent; + + /// Issue a bus reset. + async fn bus_reset(&self); + + /// Allocate channel for communication with device + /// + /// This can be a scarce resource, for one-off requests please scope the channel so it's dropped after completion + /// + /// `pre` - device is low-speed and communication is going through hub, so send PRE packet + fn alloc_channel( + &self, + addr: u8, + endpoint: &EndpointInfo, + pre: bool, + ) -> Result, HostError>; + + // Drop happens implicitly on channel-side + // / Drop allocated channel + // fn drop_channel(&self, channel: &mut Self::Channel); +} + +/// [UsbChannel] Typelevel structs and traits +// TODO: Seal traits +pub mod channel { + use super::EndpointType; + + pub trait Type { + fn ep_type() -> EndpointType; + } + pub struct Control {} + pub struct Interrupt {} + pub struct Bulk {} + pub struct Isochronous {} + impl Type for Control { + fn ep_type() -> EndpointType { + EndpointType::Control + } + } + impl Type for Interrupt { + fn ep_type() -> EndpointType { + EndpointType::Interrupt + } + } + impl Type for Bulk { + fn ep_type() -> EndpointType { + EndpointType::Bulk + } + } + impl Type for Isochronous { + fn ep_type() -> EndpointType { + EndpointType::Isochronous + } + } + + #[diagnostic::on_unimplemented(message = "This is not a CONTROL channel")] + pub trait IsControl {} + impl IsControl for Control {} + + #[diagnostic::on_unimplemented(message = "This is not a CONTROL channel")] + pub trait IsInterrupt {} + impl IsInterrupt for Interrupt {} + + pub trait Direction { + fn is_in() -> bool; + fn is_out() -> bool; + } + pub struct In {} + pub struct Out {} + pub struct InOut {} + impl Direction for In { + fn is_in() -> bool { + true + } + fn is_out() -> bool { + false + } + } + impl Direction for Out { + fn is_in() -> bool { + false + } + fn is_out() -> bool { + true + } + } + impl Direction for InOut { + fn is_in() -> bool { + true + } + fn is_out() -> bool { + true + } + } + + #[diagnostic::on_unimplemented(message = "This is not an IN channel")] + pub trait IsIn: Direction {} + impl IsIn for In {} + impl IsIn for InOut {} + + #[diagnostic::on_unimplemented(message = "This is not an OUT channel")] + pub trait IsOut: Direction {} + impl IsOut for Out {} + impl IsOut for InOut {} +} + +/// Specify the timeout of a channel +pub struct TimeoutConfig { + /// Maximum response timeout for transactions with a Data Stage + pub data_timeout: Duration, + /// Maximum response timeout for transactions without a data stage + pub standard_timeout: Duration, +} + +impl Default for TimeoutConfig { + fn default() -> Self { + TimeoutConfig { + data_timeout: Duration::from_millis(500), + standard_timeout: Duration::from_millis(50), + } + } +} + +/// ## Virtual USB Channels +/// These contain the required information to send a packet correctly to a device endpoint. +/// The information is carried with the channel on creation (see [`UsbHostDriver::alloc_channel`]) and can be changed with [`UsbChannel::retarget_channel`]. +/// +/// It is up to the HAL's driver how to implement concurrent requests, some hardware IP may allow for multiple hardware channels +/// while others may only have a single channel which needs to be multiplexed in software, while others still use DMA request linked-lists. +/// Any of these are compatible with the UsbChannel with varying degrees of sync primitives required. +pub trait UsbChannel { + /// Send IN control request + async fn control_in(&mut self, setup: &SetupPacket, buf: &mut [u8]) -> Result + where + T: channel::IsControl, + D: channel::IsIn; + + /// Send OUT control request + async fn control_out(&mut self, setup: &SetupPacket, buf: &[u8]) -> Result<(), ChannelError> + where + T: channel::IsControl, + D: channel::IsOut; + + /// Retargets channel to a new endpoint, may error if the underlying driver runs out of resources + fn retarget_channel(&mut self, addr: u8, endpoint: &EndpointInfo, pre: bool) -> Result<(), HostError>; + + /// Send IN request of type other from control + /// For interrupt channels this will return the result of the next successful interrupt poll + async fn request_in(&mut self, buf: &mut [u8]) -> Result + where + D: channel::IsIn; + + /// Send OUT request of type other from control + async fn request_out(&mut self, buf: &[u8]) -> Result<(), ChannelError> + where + D: channel::IsOut; + + /// Configure the timeouts of this channel + async fn set_timeout(&mut self, timeout: TimeoutConfig); +} diff --git a/embassy-usb-driver/src/lib.rs b/embassy-usb-driver/src/lib.rs index d204e4d858..6f724917e7 100644 --- a/embassy-usb-driver/src/lib.rs +++ b/embassy-usb-driver/src/lib.rs @@ -3,6 +3,31 @@ #![doc = include_str!("../README.md")] #![warn(missing_docs)] +pub mod host; + +/// Speed of a device or port +#[derive(Copy, Clone, Eq, PartialEq, Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum Speed { + /// 1.5 Mbit/s + Low, + /// 12 Mbit/s + Full, + /// 480 Mbit/s + High, +} + +impl Speed { + /// Provides the default max_packet_size for a given port speed + pub const fn max_packet_size(&self) -> u16 { + match self { + Speed::Low => 8, + Speed::Full => 64, + Speed::High => 8, // Minimum max_packet_size (configurable) + } + } +} + /// Direction of USB traffic. Note that in the USB standard the direction is always indicated from /// the perspective of the host, which is backward for devices, but the standard directions are used /// for consistency. @@ -110,6 +135,18 @@ pub struct EndpointInfo { pub interval_ms: u8, } +impl EndpointInfo { + /// Manually create an interface info (you should prefer deriving from parsed `EndpointDescriptor`s) + pub fn new(addr: EndpointAddress, ep_type: EndpointType, max_packet_size: u16) -> Self { + EndpointInfo { + addr, + ep_type, + max_packet_size, + interval_ms: 0, + } + } +} + /// Main USB driver trait. /// /// Implement this to add support for a new hardware platform. diff --git a/embassy-usb-synopsys-otg/Cargo.toml b/embassy-usb-synopsys-otg/Cargo.toml index 6252feaef8..d097fb27e4 100644 --- a/embassy-usb-synopsys-otg/Cargo.toml +++ b/embassy-usb-synopsys-otg/Cargo.toml @@ -20,6 +20,12 @@ critical-section = "1.1" embassy-sync = { version = "0.7.0", path = "../embassy-sync" } embassy-usb-driver = { version = "0.1.0", path = "../embassy-usb-driver" } +embassy-time = { version = "0.4.0", path = "../embassy-time" } defmt = { version = "1.0.1", optional = true } log = { version = "0.4.14", optional = true } + +[features] +default = [] +usbhost = ['alloc'] +alloc = [] diff --git a/embassy-usb-synopsys-otg/src/host.rs b/embassy-usb-synopsys-otg/src/host.rs new file mode 100644 index 0000000000..ec9623b007 --- /dev/null +++ b/embassy-usb-synopsys-otg/src/host.rs @@ -0,0 +1,1147 @@ +use core::future::poll_fn; +use core::sync::atomic::{AtomicBool, AtomicU32}; +use core::task::Poll; + +use embassy_sync::waitqueue::AtomicWaker; +use embassy_time::Timer; +use embassy_usb_driver::host::channel::{self, Direction, Type}; +use embassy_usb_driver::host::{ChannelError, DeviceEvent, HostError, SetupPacket, UsbChannel, UsbHostDriver}; +use embassy_usb_driver::{EndpointInfo, EndpointType}; + +use crate::otg_v1::vals::{Dpid, Eptyp}; +use crate::otg_v1::Otg; + +extern crate alloc; + +const HPRT_W1C_MASK: u32 = 0x2E; // Mask of interrupts inside HPRT; used to avoid eating interrupts (e.g. SOF) +const GINTST_RES_MASK: u32 = 0x08010300; + +const OTG_MAX_PIPES: usize = 8; + +/// First bit is used to indicate control pipes +static ALLOCATED_PIPES: AtomicU32 = AtomicU32::new(0); +const NEW_AW: AtomicWaker = AtomicWaker::new(); +static CH_WAKERS: [AtomicWaker; OTG_MAX_PIPES] = [NEW_AW; OTG_MAX_PIPES]; + +#[must_use = "need to hold until finished"] +#[clippy::has_significant_drop] +struct SharedChannelGuard { + channel_idx: u8, +} + +impl SharedChannelGuard { + async fn try_claim(channel_idx: u8) -> SharedChannelGuard { + loop { + let current_state = ALLOCATED_PIPES.load(core::sync::atomic::Ordering::Acquire); + if ALLOCATED_PIPES + .compare_exchange_weak( + current_state, + current_state | 1, + core::sync::atomic::Ordering::Acquire, + core::sync::atomic::Ordering::Relaxed, + ) + .is_ok() + { + break; + } + + // Claim failed; defer + embassy_time::Timer::after_micros(1).await; + } + + SharedChannelGuard { channel_idx } + } +} + +impl Drop for SharedChannelGuard { + fn drop(&mut self) { + ALLOCATED_PIPES.fetch_and(!(1 << self.channel_idx), core::sync::atomic::Ordering::AcqRel); + } +} + +struct OnDrop { + f: core::mem::MaybeUninit, +} + +impl OnDrop { + pub fn new(f: F) -> Self { + Self { + f: core::mem::MaybeUninit::new(f), + } + } + + pub fn defuse(self) { + core::mem::forget(self) + } +} + +impl Drop for OnDrop { + fn drop(&mut self) { + unsafe { self.f.as_ptr().read()() } + } +} + +/// Buffer-DMA implementation of USBOTG host driver +pub struct UsbHostBus { + regs: Otg, + dev_conn: AtomicBool, +} + +fn dma_alloc_buffer(length: usize, align: usize) -> &'static mut [T] { + let size = core::mem::size_of::(); + let layout = core::alloc::Layout::from_size_align(size * length, align).unwrap(); + unsafe { + let ptr = alloc::alloc::alloc(layout); + if ptr.is_null() { + error!("make_buffers: alloc failed"); + alloc::alloc::handle_alloc_error(layout); + } + core::slice::from_raw_parts_mut(ptr as *mut T, length) + } +} + +unsafe fn dma_dealloc_buffer(buf: &mut [T], align: usize) { + alloc::alloc::dealloc( + buf.as_mut_ptr() as *mut u8, + core::alloc::Layout::from_size_align(core::mem::size_of_val(buf), align).unwrap(), + ); +} + +/// A software-interrupt-pipe internval handler that doesn't require async +// In order to implement interrupt pipes in fifo or buffer-dma, we'll need to keep track of intervals ourselves +// luckily we have a handy frame reference in hfnum. So we can store the interval & calculated next hfnum for trigger (w/ wrapping) +struct HfnumInterruptInterval { + interval: u16, + next_hfnum: u16, + pub paused: bool, +} + +impl HfnumInterruptInterval { + pub fn new(interval: u16) -> Self { + HfnumInterruptInterval { + interval, + next_hfnum: 0, + paused: false, + } + } + + pub const fn get_interval(&self) -> u16 { + self.interval + } + + fn hfnum_delta(&self, hfnum: u16) -> u16 { + self.next_hfnum.saturating_sub(hfnum) + } + + fn reset_interval(&mut self, hfnum: u16) { + self.next_hfnum = hfnum + self.interval; + } + + fn check_and_reset_interval(&mut self, hfnum: u16) -> bool { + // Not allowed to rqeuest + if self.paused { + return false; + } + + if self.next_hfnum.wrapping_sub(hfnum) & 0x3fff > self.interval { + self.reset_interval(hfnum); + return true; + } + false + } +} + +pub struct OtgChannel { + regs: Otg, + channel_idx: u8, + interrupt_interval: Option, + buffer: &'static mut [u8], + pid: Dpid, + + device_addr: u8, + endpoint: EndpointInfo, + ls_pre: bool, + + phantom_type: core::marker::PhantomData, + phantom_dir: core::marker::PhantomData, +} + +impl OtgChannel { + #[must_use = "Expects to be further intialized using `retarget_channel`"] + fn new_alloc(otg: Otg, channel_idx: u8, buffer_size: usize) -> Self { + OtgChannel { + regs: otg, + channel_idx, + + interrupt_interval: None, + pid: Dpid::DATA0, + + device_addr: 0, + // NOTE: this will be overwritten with retarget_channel + endpoint: EndpointInfo::new(0.into(), T::ep_type(), 8), + ls_pre: false, + + buffer: dma_alloc_buffer(buffer_size, 4), + phantom_type: core::marker::PhantomData, + phantom_dir: core::marker::PhantomData, + } + } + + fn flip_pid(&mut self) { + self.pid = match self.pid { + Dpid::DATA0 => Dpid::DATA1, + Dpid::DATA1 => Dpid::DATA0, + _ => todo!("Weird state"), + } + } + + fn configure_for_endpoint(&mut self, direction_override: Option) { + self.regs.hcchar(self.channel_idx as usize).modify(|w| { + w.set_dad(self.device_addr); + w.set_lsdev(self.ls_pre); + + w.set_epnum(self.endpoint.addr.into()); + w.set_eptyp(Eptyp::from_bits(self.endpoint.ep_type as u8)); + w.set_mpsiz(self.endpoint.max_packet_size); + + w.set_epdir( + match direction_override.unwrap_or_else(|| self.endpoint.addr.direction()) { + embassy_usb_driver::Direction::In => true, + embassy_usb_driver::Direction::Out => false, + }, + ); + + w.set_chena(false); + w.set_chdis(false); + + // Clear halted status for improved error handling + self.regs.hcint(self.channel_idx as usize).write(|w| { + w.set_chh(true); + w.set_txerr(true); + w.set_bberr(true); + }); + }); + } + + fn write_setup(&mut self, setup: &SetupPacket) -> Result<(), ChannelError> { + let setup_data = setup.as_bytes(); + + self.configure_for_endpoint(Some(embassy_usb_driver::Direction::Out)); + + self.regs.hctsiz(self.channel_idx as usize).modify(|w| { + w.set_pktcnt(1); + w.set_xfrsiz(setup_data.len() as u32); + w.set_dpid(Dpid::MDATA.into()); + w.set_doping(false); + }); + + self.buffer[..setup_data.len()].copy_from_slice(setup_data); + // HCDMA gets auto-incremented so we need to set it before each tx + self.regs + .hcdma(self.channel_idx as usize) + .write(|w| w.0 = self.buffer.as_ptr() as u32); + + self.regs.gintmsk().modify(|w| { + w.set_hcim(true); + }); + + self.regs.hcchar(self.channel_idx as usize).modify(|w| { + w.set_chena(true); + w.set_chdis(false); + }); + + self.pid = Dpid::DATA1; + Ok(()) + } + + async fn wait_for_txresult(&mut self, handle_nak: bool) -> Result<(), ChannelError> { + // loop { + // Timer::after_millis(1).await; + // let hcintr = self.regs.hcint(self.channel_idx as usize).read(); + + // trace!( + // "Polling wait_for_txresult: ch_idx={}, hcintr={}", + // self.channel_idx, + // hcintr.0 + // ); + + // if hcintr.stall() { + // self.regs.hcint(self.channel_idx as usize).write(|w| w.set_stall(true)); + // return Err(ChannelError::Stall); + // } + + // if hcintr.frmor() { + // trace!("Frame overrun"); + // // self.interrupt_interval.2 = false; // Pause interrupt channel + // self.regs.hcint(self.channel_idx as usize).write(|w| w.set_frmor(true)); + // } + + // if hcintr.dterr() { + // trace!("Data toggle error"); + // // self.interrupt_interval.2 = false; // Pause interrupt channel + // self.regs.hcint(self.channel_idx as usize).write(|w| w.set_dterr(true)); + // } + + // // For OUT request ack seems like it's also counts here? + // if hcintr.xfrc() { + // // Transfer was completed + // // assert!(hcintr.ack(), "Didn't get ACK, but transfer was complete"); + + // self.regs.hcchar(self.channel_idx as usize).modify(|w| { + // // Disable channel for next trx + // w.set_chena(false); + // w.set_chdis(false); + // }); + + // self.regs.hcint(self.channel_idx as usize).write(|w| { + // w.set_xfrc(true); + // w.set_ack(true); + // }); + + // trace!("xfrc completed"); + // return Ok(()); + // } + + // // Need to check this after xfrc, since xfrc can cause a halt + // if hcintr.chh() { + // if hcintr.txerr() || hcintr.bberr() { + // warn!( + // "Got transaction error txerr={}, bberr={}, hcint={}", + // hcintr.txerr(), + // hcintr.bberr(), + // hcintr.0 + // ); + // self.regs.hcint(self.channel_idx as usize).write(|w| { + // w.set_txerr(true); + // w.set_bberr(true); + // }); + // return Err(ChannelError::BadResponse); + // } + + // if hcintr.nak() { + // trace!("Got NAK"); + // self.regs.hcint(self.channel_idx as usize).write(|w| w.set_nak(true)); + // if handle_nak { + // return Err(ChannelError::Timeout); + // } + // } + + // // Channel halted, transaction canceled + // // TODO[CherryUSB]: apparently Control endpoints do something when at INDATA state? + // trace!("Halted hcint={}", hcintr.0); + // self.regs.hcint(self.channel_idx as usize).write(|w| w.set_chh(true)); + // Err(ChannelError::Canceled)? + // } + // } + poll_fn(|cx| { + // NOTE: timeout is handled in hardware by shown by txerr, however we can't know if it was a timeout specifically + + critical_section::with(|_| { + self.regs.hcintmsk(self.channel_idx as usize).modify(|w| { + w.set_xfrcm(false); + w.set_txerrm(false); + w.set_stallm(false); + w.set_chhm(false); + }); + self.regs.haintmsk().modify(|w| { + w.0 &= !(1 << self.channel_idx as u32); + }); + }); + + let hcintr = self.regs.hcint(self.channel_idx as usize).read(); + + trace!( + "Polling wait_for_txresult: ch_idx={}, hcintr={}", + self.channel_idx, + hcintr.0 + ); + + if hcintr.stall() { + self.regs.hcint(self.channel_idx as usize).write(|w| w.set_stall(true)); + return Poll::Ready(Err(ChannelError::Stall)); + } + + if hcintr.frmor() { + trace!("Frame overrun"); + // self.interrupt_interval.2 = false; // Pause interrupt channel + self.regs.hcint(self.channel_idx as usize).write(|w| w.set_frmor(true)); + } + + if hcintr.dterr() { + trace!("Data toggle error"); + // self.interrupt_interval.2 = false; // Pause interrupt channel + self.regs.hcint(self.channel_idx as usize).write(|w| w.set_dterr(true)); + } + + // For OUT request ack seems like it's also counts here? + if hcintr.xfrc() { + // Transfer was completed + // assert!(hcintr.ack(), "Didn't get ACK, but transfer was complete"); + + self.regs.hcchar(self.channel_idx as usize).modify(|w| { + // Disable channel for next trx + w.set_chena(false); + w.set_chdis(false); + }); + + self.regs.hcint(self.channel_idx as usize).write(|w| { + w.set_xfrc(true); + w.set_ack(true); + }); + + trace!("xfrc completed"); + return Poll::Ready(Ok(())); + } + + // Need to check this after xfrc, since xfrc can cause a halt + if hcintr.chh() { + if hcintr.txerr() || hcintr.bberr() { + warn!( + "Got transaction error txerr={}, bberr={}, hcint={}", + hcintr.txerr(), + hcintr.bberr(), + hcintr.0 + ); + self.regs.hcint(self.channel_idx as usize).write(|w| { + w.set_txerr(true); + w.set_bberr(true); + }); + return Poll::Ready(Err(ChannelError::BadResponse)); + } + + if hcintr.nak() { + trace!("Got NAK"); + self.regs.hcint(self.channel_idx as usize).write(|w| w.set_nak(true)); + if handle_nak { + return Poll::Ready(Err(ChannelError::Timeout)); + } + } + + // Channel halted, transaction canceled + // TODO[CherryUSB]: apparently Control endpoints do something when at INDATA state? + trace!("Halted hcint={}", hcintr.0); + self.regs.hcint(self.channel_idx as usize).write(|w| w.set_chh(true)); + Err(ChannelError::Canceled)? + } + + CH_WAKERS[self.channel_idx as usize].register(cx.waker()); + + // Re-enable the interrupt this handled + self.regs.hcintmsk(self.channel_idx as usize).modify(|w| { + w.set_xfrcm(true); + w.set_txerrm(true); + w.set_stallm(true); + w.set_chhm(true); + }); + + self.regs.haintmsk().modify(|w| { + w.0 |= 1 << self.channel_idx as u16; + }); + + Poll::Pending + }) + .await + } +} + +impl Drop for OtgChannel { + fn drop(&mut self) { + if self.channel_idx != 0 { + ALLOCATED_PIPES.fetch_and(!(1 << self.channel_idx), core::sync::atomic::Ordering::AcqRel); + } + // Cancel any active txs & disable interrupts + + // FIXME: chdis causes next tx to fail on the same ch idx, + // but we really should chdis to avoid writing after free to the dma buffer + + // self.regs.hcchar(self.channel_idx as usize).write(|w| w.set_chdis(true)); + self.regs.hcint(self.channel_idx as usize).write(|w| w.0 = 0); + unsafe { + dma_dealloc_buffer(self.buffer, 512); + } + } +} + +impl UsbChannel for OtgChannel { + async fn control_in(&mut self, setup: &SetupPacket, buf: &mut [u8]) -> Result + where + T: channel::IsControl, + D: channel::IsIn, + { + let _ = SharedChannelGuard::try_claim(0).await; + + trace!( + "trying CTRL_IN setup={}, rt={}, buf.len()={}", + setup, + setup.request_type.bits(), + buf.len() + ); + self.write_setup(setup)?; + trace!("Wating for setup ack"); + self.wait_for_txresult(true).await?; + + self.configure_for_endpoint(Some(embassy_usb_driver::Direction::In)); + + assert!( + buf.len() == setup.length as usize, + "Expected buffer/setup length to match" + ); + + let transfer_size: u32 = setup.length as u32; + trace!( + "Finished setup; trying CTRL_IN transfer pid={}, xfrsize={}, mps={}, ep_num={}, dad={}", + self.pid as u8, + transfer_size, + self.endpoint.max_packet_size, + u8::from(self.endpoint.addr), + self.device_addr + ); + + self.regs.hctsiz(self.channel_idx as usize).modify(|w| { + w.set_pktcnt(transfer_size.div_ceil(self.endpoint.max_packet_size as u32).max(1) as u16); + w.set_xfrsiz(w.pktcnt() as u32 * self.endpoint.max_packet_size as u32); + w.set_dpid(self.pid.into()); // Control always DATA1 + w.set_doping(false); + }); + + self.regs + .hcdma(self.channel_idx as usize) + .write(|w| w.0 = self.buffer.as_ptr() as u32); + + self.regs.gintmsk().modify(|w| { + w.set_hcim(true); + }); + + self.regs.hcchar(self.channel_idx as usize).modify(|w| { + w.set_chena(true); + w.set_chdis(false); + }); + + trace!("Wating for CNTRL_IN Ack"); + self.wait_for_txresult(false).await?; + buf.copy_from_slice(&self.buffer[..transfer_size as usize]); + + // TODO: this is kind of useless since we already defined in our setup input + Ok(setup.length as usize) + } + + async fn control_out(&mut self, setup: &SetupPacket, buf: &[u8]) -> Result<(), ChannelError> + where + T: channel::IsControl, + D: channel::IsOut, + { + let _ = SharedChannelGuard::try_claim(0).await; + + self.buffer[..buf.len()].copy_from_slice(buf); + + trace!("trying CTRL_OUT setup={} rt={}", setup, setup.request_type.bits()); + self.write_setup(setup)?; + self.wait_for_txresult(true).await?; + + assert!( + buf.len() == setup.length as usize, + "Expected buffer/setup length to match" + ); + + let transfer_size: u32 = buf.len() as u32; + + if buf.is_empty() { + // Special case, write DATA_IN with zero-length + self.configure_for_endpoint(Some(embassy_usb_driver::Direction::In)); + } else { + self.configure_for_endpoint(Some(embassy_usb_driver::Direction::Out)); + } + + trace!( + "Finished setup; trying CTRL_OUT transfer pid={}, xfrsize={}, mps={}, ep_num={}, dad={}", + self.pid as u8, + transfer_size, + self.endpoint.max_packet_size, + u8::from(self.endpoint.addr), + self.device_addr + ); + + self.regs + .hcdma(self.channel_idx as usize) + .write(|w| w.0 = self.buffer.as_ptr() as u32); + + self.regs.hctsiz(self.channel_idx as usize).modify(|w| { + w.set_pktcnt(transfer_size.div_ceil(self.endpoint.max_packet_size as u32).max(1) as u16); + w.set_xfrsiz(transfer_size); + w.set_dpid(self.pid.into()); // Control always DATA1 + w.set_doping(false); + }); + + self.regs.hcchar(self.channel_idx as usize).modify(|w| { + w.set_chena(true); + w.set_chdis(false); + }); + + self.wait_for_txresult(false).await?; + + Ok(()) + } + + fn retarget_channel(&mut self, addr: u8, endpoint: &EndpointInfo, pre: bool) -> Result<(), HostError> { + self.device_addr = addr; + self.endpoint = *endpoint; + self.ls_pre = pre; + + if self.endpoint.max_packet_size as usize > self.buffer.len() { + todo!("retargeting increased buffer size; should re-alloc") + } + + // We only have a single hardware control channel, it's multiplexed using a lock + // we shouldn't change any of the registers in case a transmission is still in progress elsewhere + if endpoint.ep_type == EndpointType::Control { + return Ok(()); + } + + if endpoint.ep_type != T::ep_type() { + // TODO: add context + Err(HostError::InvalidDescriptor)? + } + + if endpoint.ep_type == EndpointType::Interrupt { + self.interrupt_interval + .replace(HfnumInterruptInterval::new(endpoint.interval_ms as u16)); + } + + self.regs.hcchar(self.channel_idx as usize).modify(|w| { + w.set_dad(addr); + w.set_lsdev(pre); + + w.set_epdir(D::is_in()); + w.set_epnum(endpoint.addr.into()); + w.set_eptyp(Eptyp::from_bits(endpoint.ep_type as u8)); + w.set_mpsiz(endpoint.max_packet_size); + + w.set_chena(false); + w.set_chdis(false); + }); + + Ok(()) + } + + async fn request_in(&mut self, buf: &mut [u8]) -> Result + where + D: channel::IsIn, + { + // Interrupt pipes should be able to resolve instantly assuming the first poll has been resolved + + loop { + trace!("trying {}_IN buf.len()={}", T::ep_type(), buf.len()); + + if T::ep_type() == EndpointType::Interrupt { + // Retry (NACK only) at polling-rate until cleared + // TODO: create a better interface for interrupts (e.g. message queue) + // This is different from hardware interrupt behaviour, as hw would discard responses if we don't handle them fast enough + // but the given interface doesn't allow for this exact behaviour + // To ensure maximum sound-ness we will only maintain the lower boundary of interrupt polling + + // SAFETY: EndpointType Interrupt should always have a interrupt_interval if retargeted + + while !self + .interrupt_interval + .as_mut() + .expect("Missing interrupt_interval; did you retarget_channel?") + .check_and_reset_interval(self.regs.hfnum().read().frnum()) + { + // NOTE: depends on speed, we assume LS/FS here + Timer::after_millis( + self.interrupt_interval + .as_ref() + .unwrap() + .hfnum_delta(self.regs.hfnum().read().frnum()) as u64, + ) + .await; + } + + trace!( + "Trying interrupt poll @{} interval={}", + self.regs.hfnum().read().frnum(), + self.interrupt_interval.as_ref().unwrap().get_interval(), + ); + } + + self.configure_for_endpoint(Some(embassy_usb_driver::Direction::In)); + + // SAFETY: mutable slices should always be accessible by dma + self.regs + .hcdma(self.channel_idx as usize) + .write(|w| w.0 = self.buffer.as_ptr() as u32); + + let transfer_size: u32 = buf.len() as u32; + self.regs.hctsiz(self.channel_idx as usize).modify(|w| { + w.set_pktcnt(transfer_size.div_ceil(self.endpoint.max_packet_size as u32).max(1) as u16); + w.set_xfrsiz(w.pktcnt() as u32 * self.endpoint.max_packet_size as u32); + // The OTGCore engine will actually toggle PIDs + // w.set_dpid(self.pid.into()); + w.set_doping(false); + }); + + self.regs.hcchar(self.channel_idx as usize).modify(|w| { + w.set_chena(true); + w.set_chdis(false); + }); + + let tx_result = self.wait_for_txresult(true).await; + + if T::ep_type() == EndpointType::Interrupt && tx_result.is_err_and(|v| v == ChannelError::Timeout) { + continue; + } + + tx_result?; + // self.flip_pid(); + + let unread_count = self.regs.hctsiz(self.channel_idx as usize).read().xfrsiz(); + let transferred_count = (transfer_size - unread_count) as usize; + buf[..transferred_count].copy_from_slice(&self.buffer[..transferred_count]); + + if T::ep_type() == EndpointType::Interrupt { + self.interrupt_interval + .as_mut() + .unwrap() + .reset_interval(self.regs.hfnum().read().frnum()) + } + + return Ok(transferred_count); + } + } + + async fn request_out(&mut self, buf: &[u8]) -> Result<(), ChannelError> + where + D: channel::IsOut, + { + // Flush fifos (UNSAFE; assumes single-thread) + // FIXME: find the fifo leak + self.regs.grstctl().write(|w| { + w.set_rxfflsh(true); + w.set_txfflsh(true); + w.set_txfnum(0b10000); // Flush all tx [RM0390] + }); + loop { + let x = self.regs.grstctl().read(); + if !x.rxfflsh() && !x.txfflsh() { + break; + } + Timer::after_micros(50).await; + } + + loop { + trace!("trying {}_OUT buf.len()={}", T::ep_type(), buf.len()); + + if T::ep_type() == EndpointType::Interrupt { + // Retry (NACK only) at polling-rate until cleared + // This is different from hardware interrupt behaviour, as hw would discard responses if we don't handle them fast enough + // but the given interface doesn't allow for this exact behaviour + // To ensure maximum sound-ness we will only maintain the lower boundary of interrupt polling + + // SAFETY: EndpointType Interrupt should always have a interrupt_interval if retargeted + + while !self + .interrupt_interval + .as_mut() + .expect("Missing interrupt_interval; did you retarget_channel?") + .check_and_reset_interval(self.regs.hfnum().read().frnum()) + { + // NOTE: depends on speed, we assume LS/FS here + Timer::after_millis( + self.interrupt_interval + .as_ref() + .unwrap() + .hfnum_delta(self.regs.hfnum().read().frnum()) as u64, + ) + .await; + } + + trace!( + "Trying interrupt poll @{} interval={}", + self.regs.hfnum().read().frnum(), + self.interrupt_interval.as_ref().unwrap().get_interval(), + ); + } + + critical_section::with(|_| { + self.configure_for_endpoint(Some(embassy_usb_driver::Direction::Out)); + + // FIXME: dma requires incoming buffer to be in ram, but immutable slice doesn't garantee this + // we handle this in case we have allocated enough to handle the entire buffer + if buf.len() < self.buffer.len() { + self.buffer[..buf.len()].copy_from_slice(buf); + + self.regs + .hcdma(self.channel_idx as usize) + .write(|w| w.0 = self.buffer.as_ptr() as u32); + } else { + self.regs + .hcdma(self.channel_idx as usize) + .write(|w| w.0 = buf.as_ptr() as u32); + } + + let transfer_size: u32 = buf.len() as u32; + self.regs.hctsiz(self.channel_idx as usize).modify(|w| { + w.set_pktcnt(transfer_size.div_ceil(self.endpoint.max_packet_size as u32).max(1) as u16); + w.set_xfrsiz(transfer_size); + // The OTGCore engine will actually toggle PIDs + // w.set_dpid(self.pid.into()); + w.set_doping(false); + }); + + core::sync::atomic::compiler_fence(core::sync::atomic::Ordering::SeqCst); + + self.regs.hcchar(self.channel_idx as usize).modify(|w| { + w.set_chena(true); + w.set_chdis(false); + }); + }); + + let tx_result = self.wait_for_txresult(true).await; + + if T::ep_type() == EndpointType::Interrupt && tx_result.is_err_and(|v| v == ChannelError::Timeout) { + continue; + } + + tx_result?; + // self.flip_pid(); + + if T::ep_type() == EndpointType::Interrupt { + self.interrupt_interval + .as_mut() + .unwrap() + .reset_interval(self.regs.hfnum().read().frnum()) + } + + return Ok(()); + } + } +} + +#[cfg(not(feature = "otg-fifo-1024"))] +const OTG_FIFO_DEPTH: usize = 256; +#[cfg(feature = "otg-fifo-1024")] +const OTG_FIFO_DEPTH: usize = 1024; + +const TX_FIFO_WORDS: usize = OTG_FIFO_DEPTH / 4; +const PTX_FIFO_WORDS: usize = OTG_FIFO_DEPTH / 8; +const RX_FIFO_WORDS: usize = OTG_FIFO_DEPTH - PTX_FIFO_WORDS - TX_FIFO_WORDS; + +const MAX_OUT_MPS: usize = TX_FIFO_WORDS * 4; +const MAX_IN_MPS: usize = (RX_FIFO_WORDS - 2) * 4; +const MAX_PTX_MPS: usize = PTX_FIFO_WORDS * 4; + +const RX_FIFO_SIZE: usize = RX_FIFO_WORDS * 4; +const TX_FIFO_SIZE: usize = TX_FIFO_WORDS * 4; + +static DEVICE_WAKER: AtomicWaker = AtomicWaker::new(); + +impl UsbHostBus { + /// Initializes and configures the Synopsys OTG core for Host-mode operation + pub fn new(otg: Otg) -> Self { + debug_assert!(otg.snpsid().read() == 0x4F54400A, "Bad synopsys id, peripheral dead?"); + + // Wait for AHB ready. + while !otg.grstctl().read().ahbidl() {} + + // Register which are not cleared by a soft-reset: + otg.gusbcfg().modify(|w| { + w.set_fhmod(true); // Force host mode + w.set_fdmod(false); // Deassert device mode + w.set_srpcap(false); + w.set_hnpcap(false); + w.set_physel(true); + // w.set_fsintf(true); + w.set_trdt(5); // Maximum + + // Use default `tocal` unless HS PHY + // w.set_tocal(7); // Maximum timeout calibration + }); + + // Perform core soft-reset + otg.grstctl().modify(|w| w.set_csrst(true)); + while otg.grstctl().read().csrst() {} + while !otg.grstctl().read().ahbidl() {} + + let bus = UsbHostBus { + regs: otg, + dev_conn: AtomicBool::new(false), + }; + + bus.init_fifo(); + + trace!("Post fifo-init: {}", otg.gintsts().read().0); + + // F429-like chips have the GCCFG.NOVBUSSENS bit + otg.gccfg_v1().modify(|w| { + // Enable internal full-speed PHY, logic is inverted + w.set_pwrdwn(true); + w.set_novbussens(true); + w.set_vbusasen(false); + w.set_vbusbsen(false); + w.set_sofouten(true); // SOF host frames + }); + + otg.pcgcctl().modify(|w| { + // Disable power down + w.set_stppclk(false); + }); + + // Setup core interrupts + otg.gintmsk().modify(|w| { + w.set_discint(true); + w.set_prtim(true); + w.set_hcim(true); + // w.set_usbrst(true); + }); + + otg.gahbcfg().modify(|w| { + w.set_gint(true); // unmask global interrupt + w.set_dmaen(true); + w.set_hbstlen(0x7); + }); + + otg.hprt().modify(|w| { + w.0 &= !HPRT_W1C_MASK; + w.set_ppwr(true); + }); + + trace!("Post init: {}", otg.gintsts().read().0); + // Clear all interrupts + // otg.gintsts().modify(|w| w.0 &= !(GINTST_RES_MASK)); + + bus + } + + /// To be called whenever the UsbHost got an interrupt or is polled + /// + /// This will check which interrupts are hit, wake correspoding tasks and mask those interrupts to prevent + /// a busy-loop of interrupts; the interrupts are expected to be re-enabled by the task if needed. + pub fn on_interrupt_or_poll(regs: Otg) { + let intr = regs.gintsts().read(); + + // trace!("[usbhostbus]: intr/polling: {}", intr.0); + if intr.discint() || intr.hprtint() { + // trace!("Prt change, waking DEVICE_WAKER"); + DEVICE_WAKER.wake(); + + regs.gintmsk().modify(|w| { + w.set_prtim(false); + w.set_discint(false); + }); + } + + let mut chintr = regs.haint().read().haint(); + while chintr != 0 { + let idx = chintr.trailing_zeros() as usize; + // trace!("Waking CH = {}, hcint={}", idx, regs.hcint(idx).read().0); + CH_WAKERS[idx].wake(); + chintr ^= 1 << idx as u16; + + // Don't trigger an interrupt for this until CH has handled the wake (or re-initialized) + regs.haintmsk().modify(|w| { + w.0 &= !(1 << idx as u32); + }) + } + // todo!("Interrupt pipe polling initiation"); + + // Clear gintsts + regs.gintsts().write(|_| {}); + } + + fn flush_fifos(&self) { + // Flush fifos (TX & PTX need to be done separately since txfnum is an indicator of which) + self.regs.grstctl().write(|w| { + w.set_rxfflsh(true); + w.set_txfflsh(true); + w.set_txfnum(0b10000); // Flush all tx [RM0390] + }); + loop { + let x = self.regs.grstctl().read(); + if !x.rxfflsh() && !x.txfflsh() { + break; + } + } + } + + fn init_fifo(&self) { + debug!("init_fifo"); + debug!("configuring rx fifo size={}", RX_FIFO_WORDS); + self.regs.grxfsiz().modify(|w| w.set_rxfd(RX_FIFO_WORDS as u16)); + // Configure TX (USB in direction) fifo size for each endpoint + let mut fifo_top = RX_FIFO_WORDS; + + debug!("configuring tx fifo, offset={}, size={}", fifo_top, TX_FIFO_WORDS); + // Non-periodic tx fifo + self.regs.hnptxfsiz().write(|w| { + w.set_fd(TX_FIFO_WORDS as u16); + w.set_sa(fifo_top as u16); + }); + fifo_top += TX_FIFO_WORDS; + + // Periodic tx fifo + self.regs.hptxfsiz().write(|w| { + w.set_fd(PTX_FIFO_WORDS as u16); + w.set_sa(fifo_top as u16); + }); + fifo_top += PTX_FIFO_WORDS; + + debug_assert!(fifo_top <= OTG_FIFO_DEPTH, "Exceeds maximum fifo allocation"); + self.flush_fifos(); + } + + fn set_port_defaults(&self) { + // Not using descriptor DMA mode + self.regs.hcfg().modify(|w| { + w.set_descdma(false); + w.set_perschedena(false); + }); + self.regs.hprt().modify(|w| { + w.0 &= !HPRT_W1C_MASK; + w.set_pena(true); + }); + + let hcfg = self.regs.hcfg().read(); + let hprt = self.regs.hprt().read(); + if hcfg.fslspcs() != hprt.pspd() { + // NOTE: Bus reset required after fslspcs change [RM0390] + warn!("Changed FSLSPCS, would require bus reset"); + self.regs.hcfg().modify(|w| { + // [CherryUSB] Align clock for Full-speed/Low-speed + w.set_fslspcs(hprt.pspd()); + }); + } + + // NOTE: hfir & fslspcs only required for FS/LS PHY + self.regs.hfir().modify(|w| { + w.set_rldctrl(true); + w.set_frivl(match hprt.pspd() { + 1 => 48000, + 2 => 6000, + _ => unreachable!("HS feature discovery not implemented"), + }) + }); + + self.init_fifo(); + } +} + +impl UsbHostDriver for UsbHostBus { + type Channel = OtgChannel; + + fn alloc_channel( + &self, + dev_addr: u8, + endpoint: &embassy_usb_driver::EndpointInfo, + pre: bool, + ) -> Result, embassy_usb_driver::host::HostError> { + trace!("Attempting to alloc channel {}, {}, {}", dev_addr, pre, endpoint); + + let new_index = if T::ep_type() == EndpointType::Control { + // Only a single control channel is available + 0 + } else { + // Atomic read-modify-write to acquire a pipe + loop { + let pipes = ALLOCATED_PIPES.load(core::sync::atomic::Ordering::Acquire); + // Ignore index 0 + let new_index = (pipes | 1).trailing_ones(); + if new_index as usize >= OTG_MAX_PIPES { + Err(HostError::OutOfChannels)?; + } + + if ALLOCATED_PIPES + .compare_exchange_weak( + pipes, + pipes | 1 << new_index, + core::sync::atomic::Ordering::Acquire, + core::sync::atomic::Ordering::Relaxed, + ) + .is_ok() + { + break new_index; + } + } + }; + + // FIXME: max_packet_size should be independent to buffer-size but due to how buffer-dma works this seems difficult to configure + // (maybe we should realloc upon receiving a larger IN/OUT request, or just ask user to provide buffers up-front?) + let mut channel = + OtgChannel::::new_alloc(self.regs, new_index as u8, (endpoint.max_packet_size * 4) as usize); + channel.retarget_channel(dev_addr, endpoint, pre)?; + Ok(channel) + } + + async fn wait_for_device_event(&self) -> embassy_usb_driver::host::DeviceEvent { + poll_fn(move |cx| { + let hprt = self.regs.hprt().read(); + trace!("Polling device event hprt={}", hprt.0); + + if !self.dev_conn.load(core::sync::atomic::Ordering::Relaxed) { + // NOTE: de-bounce skipped here; could be done interrupt poll + // crate::rom::ets_delay_us(30_000); + // let hprt = self.regs.hprt().read(); + if hprt.pcsts() { + let speed = match hprt.pspd() { + 0 => embassy_usb_driver::Speed::High, + 1 => embassy_usb_driver::Speed::Full, + 2 => embassy_usb_driver::Speed::Low, + _ => unreachable!(), + }; + self.set_port_defaults(); + debug!("Got device attached speed={}", speed); + self.dev_conn.store(true, core::sync::atomic::Ordering::Relaxed); + self.regs.gccfg_v1().modify(|w| w.set_sofouten(true)); + return Poll::Ready(DeviceEvent::Connected(speed)); + } + } else if !hprt.pcsts() { + self.dev_conn.store(false, core::sync::atomic::Ordering::Relaxed); + return Poll::Ready(DeviceEvent::Disconnected); + } + + DEVICE_WAKER.register(cx.waker()); + + // Reset interrupts + self.regs.hprt().modify(|w| { + w.0 &= !HPRT_W1C_MASK; + w.set_pocchng(true); + w.set_penchng(true); + w.set_pcdet(true); + }); + self.regs.gintsts().write(|w| { + w.set_discint(true); + }); + self.regs.gintmsk().modify(|w| { + w.set_prtim(true); + w.set_discint(true); + }); + + Poll::Pending + }) + .await + } + + async fn bus_reset(&self) { + debug!("Resetting HostBus"); + self.regs.hprt().modify(|w| { + // Port reset + w.0 &= !HPRT_W1C_MASK; + w.set_prst(true); + }); + + Timer::after_micros(15_000).await; + self.regs.hprt().modify(|w| { + w.0 &= !HPRT_W1C_MASK; + w.set_prst(false); + }); + + Timer::after_micros(15_000).await; + let hprt = self.regs.hprt().read(); + if !hprt.pena() && !hprt.pcdet() { + debug!( + "Reset doesn't seem sucessful pena: {}, pcdet: {}", + hprt.pena(), + hprt.pcdet() + ); + } + } +} diff --git a/embassy-usb-synopsys-otg/src/lib.rs b/embassy-usb-synopsys-otg/src/lib.rs index fc4428b545..c59670bfc7 100644 --- a/embassy-usb-synopsys-otg/src/lib.rs +++ b/embassy-usb-synopsys-otg/src/lib.rs @@ -6,6 +6,9 @@ // This must go FIRST so that all the other modules see its macros. mod fmt; +#[cfg(feature = "usbhost")] +pub mod host; + use core::cell::UnsafeCell; use core::future::poll_fn; use core::marker::PhantomData; @@ -1096,7 +1099,7 @@ impl<'d> embassy_usb_driver::EndpointOut for Endpoint<'d, Out> { if frame_is_odd { r.set_sd0pid_sevnfrm(true); } else { - r.set_sd1pid_soddfrm(true); + r.set_soddfrm(true); } }); } @@ -1198,7 +1201,7 @@ impl<'d> embassy_usb_driver::EndpointIn for Endpoint<'d, In> { if frame_is_odd { r.set_sd0pid_sevnfrm(true); } else { - r.set_sd1pid_soddfrm(true); + r.set_soddfrm_sd1pid(true); } }); } diff --git a/embassy-usb-synopsys-otg/src/otg_v1.rs b/embassy-usb-synopsys-otg/src/otg_v1.rs index 9e65f23154..91835946d9 100644 --- a/embassy-usb-synopsys-otg/src/otg_v1.rs +++ b/embassy-usb-synopsys-otg/src/otg_v1.rs @@ -1,4 +1,6 @@ //! Register definitions for Synopsys DesignWare USB OTG core +//! This core is well known for being poorly documented publicly, but register descriptions are available at: +//! https://www.intel.com/content/www/us/en/programmable/hps/agilex5/index_frames.html under USBOTG #![allow(missing_docs)] @@ -108,292 +110,346 @@ impl Otg { } #[doc = "Control and status register"] #[inline(always)] - pub fn gotgctl(self) -> Reg { + pub const fn gotgctl(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0usize) as _) } } #[doc = "Interrupt register"] #[inline(always)] - pub fn gotgint(self) -> Reg { + pub const fn gotgint(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x04usize) as _) } } #[doc = "AHB configuration register"] #[inline(always)] - pub fn gahbcfg(self) -> Reg { + pub const fn gahbcfg(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x08usize) as _) } } #[doc = "USB configuration register"] #[inline(always)] - pub fn gusbcfg(self) -> Reg { + pub const fn gusbcfg(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0cusize) as _) } } #[doc = "Reset register"] #[inline(always)] - pub fn grstctl(self) -> Reg { + pub const fn grstctl(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x10usize) as _) } } #[doc = "Core interrupt register"] #[inline(always)] - pub fn gintsts(self) -> Reg { + pub const fn gintsts(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x14usize) as _) } } #[doc = "Interrupt mask register"] #[inline(always)] - pub fn gintmsk(self) -> Reg { + pub const fn gintmsk(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x18usize) as _) } } #[doc = "Receive status debug read register"] #[inline(always)] - pub fn grxstsr(self) -> Reg { + pub const fn grxstsr(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x1cusize) as _) } } #[doc = "Status read and pop register"] #[inline(always)] - pub fn grxstsp(self) -> Reg { + pub const fn grxstsp(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x20usize) as _) } } #[doc = "Receive FIFO size register"] #[inline(always)] - pub fn grxfsiz(self) -> Reg { + pub const fn grxfsiz(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x24usize) as _) } } #[doc = "Endpoint 0 transmit FIFO size register (device mode)"] #[inline(always)] - pub fn dieptxf0(self) -> Reg { + pub const fn dieptxf0(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x28usize) as _) } } #[doc = "Non-periodic transmit FIFO size register (host mode)"] #[inline(always)] - pub fn hnptxfsiz(self) -> Reg { + pub const fn hnptxfsiz(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x28usize) as _) } } #[doc = "Non-periodic transmit FIFO/queue status register (host mode)"] #[inline(always)] - pub fn hnptxsts(self) -> Reg { + pub const fn hnptxsts(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x2cusize) as _) } } #[doc = "OTG I2C access register"] #[inline(always)] - pub fn gi2cctl(self) -> Reg { + pub const fn gi2cctl(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x30usize) as _) } } #[doc = "General core configuration register, for core_id 0x0000_1xxx"] #[inline(always)] - pub fn gccfg_v1(self) -> Reg { + pub const fn gccfg_v1(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x38usize) as _) } } #[doc = "General core configuration register, for core_id 0x0000_\\[23\\]xxx"] #[inline(always)] - pub fn gccfg_v2(self) -> Reg { + pub const fn gccfg_v2(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x38usize) as _) } } #[doc = "General core configuration register, for core_id 0x0000_5xxx"] #[inline(always)] - pub fn gccfg_v3(self) -> Reg { + pub const fn gccfg_v3(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x38usize) as _) } } #[doc = "Core ID register"] #[inline(always)] - pub fn cid(self) -> Reg { + pub const fn cid(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x3cusize) as _) } } + #[doc = "Synopsis ID Register"] + #[inline(always)] + pub const fn snpsid(self) -> Reg { + unsafe { Reg::from_ptr(self.ptr.add(0x40usize) as _) } + } + // TODO: https://github.com/nfeske/dwc_otg/blob/177687793c884a9ffc2e5b08acf99cf75990a88c/dwc_otg/dwc_otg_regs.h#L724 + #[doc = "User HW Config 1 register"] + #[inline(always)] + pub const fn hwcfg1(self) -> Reg { + unsafe { Reg::from_ptr(self.ptr.add(0x44usize) as _) } + } + #[doc = "User HW Config 2 register"] + #[inline(always)] + pub const fn hwcfg2(self) -> Reg { + unsafe { Reg::from_ptr(self.ptr.add(0x48usize) as _) } + } + #[doc = "User HW Config 3 register"] + #[inline(always)] + pub const fn hwcfg3(self) -> Reg { + unsafe { Reg::from_ptr(self.ptr.add(0x4cusize) as _) } + } + #[doc = "User HW Config 4 register"] + #[inline(always)] + pub const fn hwcfg4(self) -> Reg { + unsafe { Reg::from_ptr(self.ptr.add(0x50usize) as _) } + } #[doc = "OTG core LPM configuration register"] #[inline(always)] - pub fn glpmcfg(self) -> Reg { + pub const fn glpmcfg(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x54usize) as _) } } + #[doc = "Global PowerDn Register"] + #[inline(always)] + pub const fn gpwrdn(self) -> Reg { + unsafe { Reg::from_ptr(self.ptr.add(0x58usize) as _) } + } + #[doc = "Global DFIFO SW Config Register"] + #[inline(always)] + pub const fn gdfifocfg(self) -> Reg { + unsafe { Reg::from_ptr(self.ptr.add(0x5cusize) as _) } + } + #[doc = "ADP (Attach Detection Protocol) Control Register"] + #[inline(always)] + pub const fn adpctl(self) -> Reg { + unsafe { Reg::from_ptr(self.ptr.add(0x60usize) as _) } + } #[doc = "Host periodic transmit FIFO size register"] #[inline(always)] - pub fn hptxfsiz(self) -> Reg { + pub const fn hptxfsiz(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0100usize) as _) } } #[doc = "Device IN endpoint transmit FIFO size register"] #[inline(always)] - pub fn dieptxf(self, n: usize) -> Reg { - assert!(n < 7usize); + pub const fn dieptxf(self, n: usize) -> Reg { + core::assert!(n < 7usize); unsafe { Reg::from_ptr(self.ptr.add(0x0104usize + n * 4usize) as _) } } #[doc = "Host configuration register"] #[inline(always)] - pub fn hcfg(self) -> Reg { + pub const fn hcfg(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0400usize) as _) } } #[doc = "Host frame interval register"] #[inline(always)] - pub fn hfir(self) -> Reg { + pub const fn hfir(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0404usize) as _) } } #[doc = "Host frame number/frame time remaining register"] #[inline(always)] - pub fn hfnum(self) -> Reg { + pub const fn hfnum(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0408usize) as _) } } #[doc = "Periodic transmit FIFO/queue status register"] #[inline(always)] - pub fn hptxsts(self) -> Reg { + pub const fn hptxsts(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0410usize) as _) } } #[doc = "Host all channels interrupt register"] #[inline(always)] - pub fn haint(self) -> Reg { + pub const fn haint(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0414usize) as _) } } + #[doc = "Host Frame Scheduling List Register"] + #[inline(always)] + pub const fn hflbaddr(self) -> Reg { + unsafe { Reg::from_ptr(self.ptr.add(0x41cusize) as _) } + } #[doc = "Host all channels interrupt mask register"] #[inline(always)] - pub fn haintmsk(self) -> Reg { + pub const fn haintmsk(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0418usize) as _) } } #[doc = "Host port control and status register"] #[inline(always)] - pub fn hprt(self) -> Reg { + pub const fn hprt(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0440usize) as _) } } #[doc = "Host channel characteristics register"] #[inline(always)] - pub fn hcchar(self, n: usize) -> Reg { - assert!(n < 12usize); + pub const fn hcchar(self, n: usize) -> Reg { + core::assert!(n < 12usize); unsafe { Reg::from_ptr(self.ptr.add(0x0500usize + n * 32usize) as _) } } #[doc = "Host channel split control register"] #[inline(always)] - pub fn hcsplt(self, n: usize) -> Reg { - assert!(n < 12usize); + pub const fn hcsplt(self, n: usize) -> Reg { + core::assert!(n < 12usize); unsafe { Reg::from_ptr(self.ptr.add(0x0504usize + n * 32usize) as _) } } #[doc = "Host channel interrupt register"] #[inline(always)] - pub fn hcint(self, n: usize) -> Reg { - assert!(n < 12usize); + pub const fn hcint(self, n: usize) -> Reg { + core::assert!(n < 12usize); unsafe { Reg::from_ptr(self.ptr.add(0x0508usize + n * 32usize) as _) } } #[doc = "Host channel mask register"] #[inline(always)] - pub fn hcintmsk(self, n: usize) -> Reg { - assert!(n < 12usize); + pub const fn hcintmsk(self, n: usize) -> Reg { + core::assert!(n < 12usize); unsafe { Reg::from_ptr(self.ptr.add(0x050cusize + n * 32usize) as _) } } #[doc = "Host channel transfer size register"] #[inline(always)] - pub fn hctsiz(self, n: usize) -> Reg { - assert!(n < 12usize); + pub const fn hctsiz(self, n: usize) -> Reg { + core::assert!(n < 12usize); unsafe { Reg::from_ptr(self.ptr.add(0x0510usize + n * 32usize) as _) } } - #[doc = "Host channel DMA address register"] + #[doc = "Host channel DMA address register (config for scatter/gather, ptr for buffer-dma)"] #[inline(always)] - pub fn hcdma(self, n: usize) -> Reg { - assert!(n < 12usize); + pub const fn hcdma(self, n: usize) -> Reg { + core::assert!(n < 12usize); unsafe { Reg::from_ptr(self.ptr.add(0x0514usize + n * 32usize) as _) } } + #[doc = "Host channel DMA address register (addr buffer for current transfer; used to debug ddma)"] + #[inline(always)] + pub const fn hcdmab(self, n: usize) -> Reg { + core::assert!(n < 12usize); + unsafe { Reg::from_ptr(self.ptr.add(0x051cusize + n * 32usize) as _) } + } #[doc = "Device configuration register"] #[inline(always)] - pub fn dcfg(self) -> Reg { + pub const fn dcfg(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0800usize) as _) } } #[doc = "Device control register"] #[inline(always)] - pub fn dctl(self) -> Reg { + pub const fn dctl(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0804usize) as _) } } #[doc = "Device status register"] #[inline(always)] - pub fn dsts(self) -> Reg { + pub const fn dsts(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0808usize) as _) } } #[doc = "Device IN endpoint common interrupt mask register"] #[inline(always)] - pub fn diepmsk(self) -> Reg { + pub const fn diepmsk(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0810usize) as _) } } #[doc = "Device OUT endpoint common interrupt mask register"] #[inline(always)] - pub fn doepmsk(self) -> Reg { + pub const fn doepmsk(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0814usize) as _) } } #[doc = "Device all endpoints interrupt register"] #[inline(always)] - pub fn daint(self) -> Reg { + pub const fn daint(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0818usize) as _) } } #[doc = "All endpoints interrupt mask register"] #[inline(always)] - pub fn daintmsk(self) -> Reg { + pub const fn daintmsk(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x081cusize) as _) } } #[doc = "Device VBUS discharge time register"] #[inline(always)] - pub fn dvbusdis(self) -> Reg { + pub const fn dvbusdis(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0828usize) as _) } } #[doc = "Device VBUS pulsing time register"] #[inline(always)] - pub fn dvbuspulse(self) -> Reg { + pub const fn dvbuspulse(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x082cusize) as _) } } #[doc = "Device IN endpoint FIFO empty interrupt mask register"] #[inline(always)] - pub fn diepempmsk(self) -> Reg { + pub const fn diepempmsk(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0834usize) as _) } } #[doc = "Device IN endpoint control register"] #[inline(always)] - pub fn diepctl(self, n: usize) -> Reg { - assert!(n < 16usize); + pub const fn diepctl(self, n: usize) -> Reg { + core::assert!(n < 16usize); unsafe { Reg::from_ptr(self.ptr.add(0x0900usize + n * 32usize) as _) } } #[doc = "Device IN endpoint interrupt register"] #[inline(always)] - pub fn diepint(self, n: usize) -> Reg { - assert!(n < 16usize); + pub const fn diepint(self, n: usize) -> Reg { + core::assert!(n < 16usize); unsafe { Reg::from_ptr(self.ptr.add(0x0908usize + n * 32usize) as _) } } #[doc = "Device IN endpoint transfer size register"] #[inline(always)] - pub fn dieptsiz(self, n: usize) -> Reg { - assert!(n < 16usize); + pub const fn dieptsiz(self, n: usize) -> Reg { + core::assert!(n < 16usize); unsafe { Reg::from_ptr(self.ptr.add(0x0910usize + n * 32usize) as _) } } #[doc = "Device IN endpoint transmit FIFO status register"] #[inline(always)] - pub fn dtxfsts(self, n: usize) -> Reg { - assert!(n < 16usize); + pub const fn dtxfsts(self, n: usize) -> Reg { + core::assert!(n < 16usize); unsafe { Reg::from_ptr(self.ptr.add(0x0918usize + n * 32usize) as _) } } #[doc = "Device OUT endpoint control register"] #[inline(always)] - pub fn doepctl(self, n: usize) -> Reg { - assert!(n < 16usize); + pub const fn doepctl(self, n: usize) -> Reg { + core::assert!(n < 16usize); unsafe { Reg::from_ptr(self.ptr.add(0x0b00usize + n * 32usize) as _) } } #[doc = "Device OUT endpoint interrupt register"] #[inline(always)] - pub fn doepint(self, n: usize) -> Reg { - assert!(n < 16usize); + pub const fn doepint(self, n: usize) -> Reg { + core::assert!(n < 16usize); unsafe { Reg::from_ptr(self.ptr.add(0x0b08usize + n * 32usize) as _) } } #[doc = "Device OUT endpoint transfer size register"] #[inline(always)] - pub fn doeptsiz(self, n: usize) -> Reg { - assert!(n < 16usize); + pub const fn doeptsiz(self, n: usize) -> Reg { + core::assert!(n < 16usize); unsafe { Reg::from_ptr(self.ptr.add(0x0b10usize + n * 32usize) as _) } } #[doc = "Device OUT/IN endpoint DMA address register"] #[inline(always)] - pub fn doepdma(self, n: usize) -> Reg { - assert!(n < 16usize); + pub const fn doepdma(self, n: usize) -> Reg { + core::assert!(n < 16usize); unsafe { Reg::from_ptr(self.ptr.add(0x0b14usize + n * 32usize) as _) } } #[doc = "Power and clock gating control register"] #[inline(always)] - pub fn pcgcctl(self) -> Reg { + pub const fn pcgcctl(self) -> Reg { unsafe { Reg::from_ptr(self.ptr.add(0x0e00usize) as _) } } #[doc = "Device endpoint / host channel FIFO register"] #[inline(always)] - pub fn fifo(self, n: usize) -> Reg { - assert!(n < 16usize); + pub const fn fifo(self, n: usize) -> Reg { + core::assert!(n < 16usize); unsafe { Reg::from_ptr(self.ptr.add(0x1000usize + n * 4096usize) as _) } } } pub mod regs { + use super::vals::FrameListLen; + #[doc = "Core ID register"] #[repr(transparent)] #[derive(Copy, Clone, Eq, PartialEq)] @@ -408,7 +464,7 @@ pub mod regs { #[doc = "Product ID field"] #[inline(always)] pub fn set_product_id(&mut self, val: u32) { - self.0 = (self.0 & !(0xffff_ffff << 0usize)) | (((val as u32) & 0xffff_ffff) << 0usize); + self.0 = val } } impl Default for Cid { @@ -417,6 +473,160 @@ pub mod regs { Cid(0) } } + + #[doc = "Core ID register"] + #[repr(transparent)] + #[derive(Copy, Clone, Eq, PartialEq)] + pub struct AdpCtl(pub u32); + impl AdpCtl { + #[doc = "Probe Discharge time (times for TADP_DSCHG)"] + #[inline(always)] + pub const fn prb_dschg(&self) -> u8 { + self.0 as u8 & 0b11 + } + #[doc = "Probe Discharge time (times for TADP_DSCHG)"] + #[inline(always)] + pub fn set_prb_dschg(&mut self, val: u8) { + self.0 = (self.0 & !(0b11u32)) | ((val as u32) & 0b11); + } + #[doc = "Probe Delta (resolution for RTIM)"] + #[inline(always)] + pub const fn prb_delta(&self) -> u8 { + (self.0 >> 2) as u8 & 0b11 + } + #[doc = "Probe Delta (resolution for RTIM)"] + #[inline(always)] + pub fn set_prb_delta(&mut self, val: u8) { + self.0 = (self.0 & !(0b11 << 2)) | (((val as u32) & 0b11) << 2usize); + } + #[doc = "Probe Period (TADP_PRD)"] + #[inline(always)] + pub const fn prb_per(&self) -> u8 { + (self.0 >> 4) as u8 & 0b11 + } + #[doc = "Probe Period (TADP_PRD)"] + #[inline(always)] + pub fn set_prb_per(&mut self, val: u8) { + self.0 = (self.0 & !(0b11 << 4)) | (((val as u32) & 0b11) << 4usize); + } + #[doc = "Probe Period (TADP_PRD)"] + #[inline(always)] + pub const fn rtim(&self) -> u16 { + (self.0 >> 6) as u16 & 0x7ff + } + #[doc = "Probe Period (TADP_PRD)"] + #[inline(always)] + pub fn set_rtim(&mut self, val: u16) { + self.0 = (self.0 & !(0x7ff << 6)) | (((val as u32) & 0x7ff) << 6usize); + } + #[doc = "Enable Probe"] + #[inline(always)] + pub const fn enaprb(&self) -> bool { + (self.0 >> 17) & 0b1 != 0 + } + #[doc = "Enable Probe"] + #[inline(always)] + pub fn set_enaprb(&mut self, val: bool) { + self.0 = (self.0 & !(0b1 << 17)) | ((val as u32) << 17usize); + } + #[doc = "Enable Sense"] + #[inline(always)] + pub const fn enasns(&self) -> bool { + (self.0 >> 18) & 0b1 != 0 + } + #[doc = "Enable Sense"] + #[inline(always)] + pub fn set_enasns(&mut self, val: bool) { + self.0 = (self.0 & !(0b1 << 18)) | ((val as u32) << 18usize); + } + #[doc = "ADP Reset"] + #[inline(always)] + pub const fn adpres(&self) -> bool { + (self.0 >> 19) & 0b1 != 0 + } + #[doc = "ADP Reset"] + #[inline(always)] + pub fn set_adpres(&mut self, val: bool) { + self.0 = (self.0 & !(0b1 << 19)) | ((val as u32) << 19usize); + } + #[doc = "ADP Enable"] + #[inline(always)] + pub const fn adpen(&self) -> bool { + (self.0 >> 20) & 0b1 != 0 + } + #[doc = "ADP Enable"] + #[inline(always)] + pub fn set_adpen(&mut self, val: bool) { + self.0 = (self.0 & !(0b1 << 20)) | ((val as u32) << 20usize); + } + #[doc = "ADP Probe Interrupt Enable"] + #[inline(always)] + pub const fn adp_prb_int(&self) -> bool { + (self.0 >> 21) & 0b1 != 0 + } + #[doc = "ADP Probe Interrupt Enable"] + #[inline(always)] + pub fn set_adp_prb_int(&mut self, val: bool) { + self.0 = (self.0 & !(0b1 << 21)) | ((val as u32) << 21usize); + } + #[doc = "ADP Sense Interrupt Enable"] + #[inline(always)] + pub const fn adp_sns_int(&self) -> bool { + (self.0 >> 22) & 0b1 != 0 + } + #[doc = "ADP Sense Interrupt Enable"] + #[inline(always)] + pub fn set_adp_sns_int(&mut self, val: bool) { + self.0 = (self.0 & !(0b1 << 22)) | ((val as u32) << 22usize); + } + #[doc = "ADP Timeout Interrupt Enable"] + #[inline(always)] + pub const fn adp_tmout_int(&self) -> bool { + (self.0 >> 23) & 0b1 != 0 + } + #[doc = "ADP Timeout Interrupt Enable"] + #[inline(always)] + pub fn set_adp_tmout_int(&mut self, val: bool) { + self.0 = (self.0 & !(0b1 << 23)) | ((val as u32) << 23usize); + } + #[doc = "ADP Probe Interrupt Mask"] + #[inline(always)] + pub const fn adp_prb_msk(&self) -> bool { + (self.0 >> 24) & 0b1 != 0 + } + #[doc = "ADP Probe Interrupt Mask"] + #[inline(always)] + pub fn set_adp_prb_msk(&mut self, val: bool) { + self.0 = (self.0 & !(0b1 << 24)) | ((val as u32) << 24usize); + } + #[doc = "ADP Timeout Interrupt Mask"] + #[inline(always)] + pub const fn adp_tmout_msk(&self) -> bool { + (self.0 >> 25) & 0b1 != 0 + } + #[doc = "ADP Timeout Interrupt Mask"] + #[inline(always)] + pub fn set_adp_tmout_msk(&mut self, val: bool) { + self.0 = (self.0 & !(0b1 << 25)) | ((val as u32) << 25usize); + } + #[doc = "Access Request"] + #[inline(always)] + pub const fn ar(&self) -> u8 { + (self.0 >> 26) as u8 & 0b11 + } + #[doc = "Access Request"] + #[inline(always)] + pub fn set_ar(&mut self, val: u8) { + self.0 = (self.0 & !(0b11 << 26)) | ((val as u32) << 26usize); + } + } + impl Default for AdpCtl { + #[inline(always)] + fn default() -> AdpCtl { + AdpCtl(0) + } + } + #[doc = "Device all endpoints interrupt register"] #[repr(transparent)] #[derive(Copy, Clone, Eq, PartialEq)] @@ -800,15 +1010,15 @@ pub mod regs { pub fn set_sd0pid_sevnfrm(&mut self, val: bool) { self.0 = (self.0 & !(0x01 << 28usize)) | (((val as u32) & 0x01) << 28usize); } - #[doc = "SD1PID/SODDFRM"] + #[doc = "SODDFRM/SD1PID"] #[inline(always)] - pub const fn sd1pid_soddfrm(&self) -> bool { + pub const fn soddfrm_sd1pid(&self) -> bool { let val = (self.0 >> 29usize) & 0x01; val != 0 } - #[doc = "SD1PID/SODDFRM"] + #[doc = "SODDFRM/SD1PID"] #[inline(always)] - pub fn set_sd1pid_soddfrm(&mut self, val: bool) { + pub fn set_soddfrm_sd1pid(&mut self, val: bool) { self.0 = (self.0 & !(0x01 << 29usize)) | (((val as u32) & 0x01) << 29usize); } #[doc = "EPDIS"] @@ -1035,6 +1245,7 @@ pub mod regs { pub fn set_xfrsiz(&mut self, val: u32) { self.0 = (self.0 & !(0x0007_ffff << 0usize)) | (((val as u32) & 0x0007_ffff) << 0usize); } + #[doc = "Packet count"] #[inline(always)] pub const fn pktcnt(&self) -> u16 { @@ -1179,15 +1390,15 @@ pub mod regs { pub fn set_sd0pid_sevnfrm(&mut self, val: bool) { self.0 = (self.0 & !(0x01 << 28usize)) | (((val as u32) & 0x01) << 28usize); } - #[doc = "SD1PID/SODDFRM"] + #[doc = "SODDFRM"] #[inline(always)] - pub const fn sd1pid_soddfrm(&self) -> bool { + pub const fn soddfrm(&self) -> bool { let val = (self.0 >> 29usize) & 0x01; val != 0 } - #[doc = "SD1PID/SODDFRM"] + #[doc = "SODDFRM"] #[inline(always)] - pub fn set_sd1pid_soddfrm(&mut self, val: bool) { + pub fn set_soddfrm(&mut self, val: bool) { self.0 = (self.0 & !(0x01 << 29usize)) | (((val as u32) & 0x01) << 29usize); } #[doc = "EPDIS"] @@ -1526,7 +1737,7 @@ pub mod regs { #[doc = "Data"] #[inline(always)] pub fn set_data(&mut self, val: u32) { - self.0 = (self.0 & !(0xffff_ffff << 0usize)) | (((val as u32) & 0xffff_ffff) << 0usize); + self.0 = val } } impl Default for Fifo { @@ -2647,6 +2858,18 @@ pub mod regs { pub fn set_datafsusp(&mut self, val: bool) { self.0 = (self.0 & !(0x01 << 22usize)) | (((val as u32) & 0x01) << 22usize); } + #[doc = "Reset detected"] + #[inline(always)] + pub const fn resetdet(&self) -> bool { + let val = (self.0 >> 23usize) & 0x01; + val != 0 + } + #[doc = "Reset detected"] + #[inline(always)] + pub fn set_resetdet(&mut self, val: bool) { + self.0 = (self.0 & !(0x01 << 23usize)) | (((val as u32) & 0x01) << 23usize); + } + #[doc = "Host port interrupt"] #[inline(always)] pub const fn hprtint(&self) -> bool { @@ -3653,6 +3876,28 @@ pub mod regs { Haintmsk(0) } } + #[doc = "Host frame list base address"] + #[repr(transparent)] + #[derive(Copy, Clone, Eq, PartialEq)] + pub struct Hflbaddr(pub u32); + impl Hflbaddr { + #[doc = "Host frame list base address"] + #[inline(always)] + pub const fn hflbaddr(&self) -> u32 { + self.0 + } + #[doc = "Host frame list base address"] + #[inline(always)] + pub fn set_hflbaddr(&mut self, val: u32) { + self.0 = val + } + } + impl Default for Hflbaddr { + #[inline(always)] + fn default() -> Hflbaddr { + Hflbaddr(0) + } + } #[doc = "Host channel characteristics register"] #[repr(transparent)] #[derive(Copy, Clone, Eq, PartialEq)] @@ -3737,6 +3982,7 @@ pub mod regs { } #[doc = "Odd frame"] #[inline(always)] + /// Indicates to the USBOTG core that the (iso or intr) transaction must be performed on an odd (micro)frame pub const fn oddfrm(&self) -> bool { let val = (self.0 >> 29usize) & 0x01; val != 0 @@ -3802,6 +4048,38 @@ pub mod regs { pub fn set_fslss(&mut self, val: bool) { self.0 = (self.0 & !(0x01 << 2usize)) | (((val as u32) & 0x01) << 2usize); } + #[doc = "Period scheduling enable"] + #[inline(always)] + pub fn set_perschedena(&mut self, val: bool) { + self.0 = (self.0 & !(0x01 << 26)) | ((val as u32) << 26); + } + #[doc = "Period scheduling enable"] + #[inline(always)] + pub fn perschedena(&self) -> bool { + let val = (self.0 >> 26) & 0x1; + val != 0 + } + #[doc = "Descriptor DMA-mode enable (qtd)"] + #[inline(always)] + pub fn descdma(&self) -> bool { + (self.0 << 23) & 0x1 != 0 + } + #[doc = "Descriptor DMA-mode enable (qtd)"] + #[inline(always)] + pub fn set_descdma(&mut self, val: bool) { + self.0 = (self.0 & !(0x1 << 23)) | (val as u32) << 23 + } + #[doc = "Frame list length (x+3 pow 2)"] + #[inline(always)] + pub fn frlistlen(&self) -> FrameListLen { + let val = (self.0 << 24) & 0x2; + (val as u8).into() + } + #[doc = "Frame list length (x+3 pow 2)"] + #[inline(always)] + pub fn set_frlistlen(&mut self, val: FrameListLen) { + self.0 = (self.0 & !(0x2 << 24)) | ((val as u32) & 0x2) << 24 + } } impl Default for Hcfg { #[inline(always)] @@ -4047,17 +4325,37 @@ pub mod regs { #[derive(Copy, Clone, Eq, PartialEq)] pub struct Hctsiz(pub u32); impl Hctsiz { - #[doc = "Transfer size"] + #[doc = "Transfer size for non-isochronuous/interrupt pipes"] #[inline(always)] pub const fn xfrsiz(&self) -> u32 { let val = (self.0 >> 0usize) & 0x0007_ffff; val as u32 } - #[doc = "Transfer size"] + #[doc = "Transfer size for non-isochronuous/interrupt pipes"] #[inline(always)] pub fn set_xfrsiz(&mut self, val: u32) { self.0 = (self.0 & !(0x0007_ffff << 0usize)) | (((val as u32) & 0x0007_ffff) << 0usize); } + #[doc = "NTD descriptor list length for isochronuous & interrupt pipes (xfersiz[15:8], note val+1 is actual length)"] + #[inline(always)] + pub const fn ntdl(&self) -> u8 { + (self.0 >> 8) as u8 + } + #[doc = "NTD descriptor list length for isochronuous & interrupt pipes (xfrsiz[15:8], note val-1 is actual length)"] + #[inline(always)] + pub fn set_ntdl(&mut self, val: u8) { + self.0 = (self.0 & !(0xFF << 8)) | ((val as u32) << 8) + } + #[doc = "Schedule info for isochronuous & interrupt pipes (xfrsiz[7:0])"] + #[inline(always)] + pub const fn schedinfo(&self) -> u8 { + self.0 as u8 + } + #[doc = "Schedule info for isochronuous & interrupt pipes (xfrsiz[7:0])"] + #[inline(always)] + pub fn set_schedinfo(&mut self, val: u8) { + self.0 = (self.0 & !(0xFF << 8)) | ((val as u32) << 8) + } #[doc = "Packet count"] #[inline(always)] pub const fn pktcnt(&self) -> u16 { @@ -4080,6 +4378,17 @@ pub mod regs { pub fn set_dpid(&mut self, val: u8) { self.0 = (self.0 & !(0x03 << 29usize)) | (((val as u32) & 0x03) << 29usize); } + #[doc = "Do Ping"] + #[inline(always)] + pub const fn doping(&self) -> bool { + let val = (self.0 >> 31usize) & 0x01; + val != 0 + } + #[doc = "Do Ping"] + #[inline(always)] + pub fn set_doping(&mut self, val: bool) { + self.0 = (self.0 & !(0x01 << 31usize)) | (((val as u32) & 0x01) << 31usize); + } } impl Default for Hctsiz { #[inline(always)] @@ -4087,6 +4396,40 @@ pub mod regs { Hctsiz(0) } } + #[doc = "Host channel DMA config register"] + #[repr(transparent)] + #[derive(Copy, Clone, Eq, PartialEq)] + pub struct Hcdma(pub u32); + impl Hcdma { + #[doc = "Current QTD (transfer descriptor) index"] + #[inline(always)] + pub const fn cqtd(&self) -> u8 { + ((self.0 >> 3) & 0x3F) as u8 + } + #[doc = "Current QTD (transfer descriptor) index"] + #[inline(always)] + pub fn set_cqtd(&mut self, val: u8) { + self.0 = (self.0 & !(0x3f << 3)) | (val as u32 & 0x3F) << 3; + } + #[doc = "QTD list base address"] + #[inline(always)] + pub const fn qtdaddr(&self) -> u32 { + self.0 & 0xFFFFFE00 + } + #[doc = "QTD list base address"] + #[inline(always)] + pub fn set_qtdaddr(&mut self, val: u32) { + core::debug_assert!(val & 0xFFFFFE00 == val, "QTD list needs to be 512 byte aligned"); + self.0 = (self.0 & !0xFFFFFE00) | val; + } + } + impl Default for Hcdma { + #[inline(always)] + fn default() -> Hcdma { + Hcdma(0) + } + } + #[doc = "Host frame interval register"] #[repr(transparent)] #[derive(Copy, Clone, Eq, PartialEq)] @@ -4103,6 +4446,16 @@ pub mod regs { pub fn set_frivl(&mut self, val: u16) { self.0 = (self.0 & !(0xffff << 0usize)) | (((val as u32) & 0xffff) << 0usize); } + #[doc = "Dynamic Loading Control"] + #[inline(always)] + pub const fn rldctrl(&self) -> bool { + (self.0 >> 16usize) & 0x1 != 0 + } + #[doc = "Dynamic Loading Control"] + #[inline(always)] + pub fn set_rldctrl(&mut self, val: bool) { + self.0 = (self.0 & !(0b1 << 16usize)) | ((val as u32) << 16usize); + } } impl Default for Hfir { #[inline(always)] @@ -4222,7 +4575,7 @@ pub mod regs { let val = (self.0 >> 2usize) & 0x01; val != 0 } - #[doc = "Port enable"] + #[doc = "Port enable (write 1 to disable)"] #[inline(always)] pub fn set_pena(&mut self, val: bool) { self.0 = (self.0 & !(0x01 << 2usize)) | (((val as u32) & 0x01) << 2usize); @@ -4436,6 +4789,47 @@ pub mod regs { } } pub mod vals { + #[repr(u8)] + #[derive(Copy, Clone, Eq, PartialEq, Ord, PartialOrd)] + #[allow(non_camel_case_types)] + pub enum FrameListLen { + LEN8 = 0x0, + LEN16 = 0x1, + LEN32 = 0x2, + LEN64 = 0x3, + } + impl FrameListLen { + #[inline(always)] + pub const fn from_bits(val: u8) -> FrameListLen { + unsafe { core::mem::transmute(val & 0x03) } + } + #[inline(always)] + pub const fn to_bits(self) -> u8 { + self as u8 + } + #[inline(always)] + pub const fn as_value(self) -> u8 { + match self { + Self::LEN8 => 8, + Self::LEN16 => 16, + Self::LEN32 => 32, + Self::LEN64 => 64, + } + } + } + impl From for FrameListLen { + #[inline(always)] + fn from(val: u8) -> FrameListLen { + FrameListLen::from_bits(val) + } + } + impl From for u8 { + #[inline(always)] + fn from(val: FrameListLen) -> u8 { + FrameListLen::to_bits(val) + } + } + #[repr(u8)] #[derive(Copy, Clone, Eq, PartialEq, Ord, PartialOrd)] #[allow(non_camel_case_types)] @@ -4452,7 +4846,7 @@ pub mod vals { } #[inline(always)] pub const fn to_bits(self) -> u8 { - unsafe { core::mem::transmute(self) } + self as u8 } } impl From for Dpid { @@ -4517,7 +4911,7 @@ pub mod vals { } #[inline(always)] pub const fn to_bits(self) -> u8 { - unsafe { core::mem::transmute(self) } + self as u8 } } impl From for Eptyp { @@ -4552,7 +4946,7 @@ pub mod vals { } #[inline(always)] pub const fn to_bits(self) -> u8 { - unsafe { core::mem::transmute(self) } + self as u8 } } impl From for Pfivl { @@ -4600,7 +4994,7 @@ pub mod vals { } #[inline(always)] pub const fn to_bits(self) -> u8 { - unsafe { core::mem::transmute(self) } + self as u8 } } impl From for Pktstsd { @@ -4647,7 +5041,7 @@ pub mod vals { } #[inline(always)] pub const fn to_bits(self) -> u8 { - unsafe { core::mem::transmute(self) } + self as u8 } } impl From for Pktstsh { diff --git a/embassy-usb/Cargo.toml b/embassy-usb/Cargo.toml index 31fd1c1e0f..ba245679ec 100644 --- a/embassy-usb/Cargo.toml +++ b/embassy-usb/Cargo.toml @@ -50,12 +50,16 @@ embassy-futures = { version = "0.1.0", path = "../embassy-futures" } embassy-usb-driver = { version = "0.1.0", path = "../embassy-usb-driver" } embassy-sync = { version = "0.7.0", path = "../embassy-sync" } embassy-net-driver-channel = { version = "0.3.0", path = "../embassy-net-driver-channel" } +embassy-time = { version = "0.4.0", path="../embassy-time" } defmt = { version = "1", optional = true } log = { version = "0.4.14", optional = true } heapless = "0.8" embedded-io-async = "0.6.1" +bitflags = "2.6.0" # for HID usbd-hid = { version = "0.8.1", optional = true } ssmarshal = { version = "1.0", default-features = false, optional = true } +zerocopy = { version = "0.8.2", features = ["derive"] } +bit_field = "0.10.2" diff --git a/embassy-usb/src/descriptor_reader.rs b/embassy-usb/src/descriptor_reader.rs deleted file mode 100644 index abb4b379ef..0000000000 --- a/embassy-usb/src/descriptor_reader.rs +++ /dev/null @@ -1,111 +0,0 @@ -use crate::descriptor::descriptor_type; -use crate::driver::EndpointAddress; -use crate::types::InterfaceNumber; - -#[derive(Copy, Clone, PartialEq, Eq, Debug)] -#[cfg_attr(feature = "defmt", derive(defmt::Format))] -pub struct ReadError; - -pub struct Reader<'a> { - data: &'a [u8], -} - -impl<'a> Reader<'a> { - pub const fn new(data: &'a [u8]) -> Self { - Self { data } - } - - pub const fn eof(&self) -> bool { - self.data.is_empty() - } - - pub fn read(&mut self) -> Result<[u8; N], ReadError> { - let n = self.data.get(0..N).ok_or(ReadError)?; - self.data = &self.data[N..]; - Ok(n.try_into().unwrap()) - } - - pub fn read_u8(&mut self) -> Result { - Ok(u8::from_le_bytes(self.read()?)) - } - pub fn read_u16(&mut self) -> Result { - Ok(u16::from_le_bytes(self.read()?)) - } - - pub fn read_slice(&mut self, len: usize) -> Result<&'a [u8], ReadError> { - let res = self.data.get(0..len).ok_or(ReadError)?; - self.data = &self.data[len..]; - Ok(res) - } - - pub fn read_descriptors(&mut self) -> DescriptorIter<'_, 'a> { - DescriptorIter { r: self } - } -} - -pub struct DescriptorIter<'a, 'b> { - r: &'a mut Reader<'b>, -} - -impl<'a, 'b> Iterator for DescriptorIter<'a, 'b> { - type Item = Result<(u8, Reader<'a>), ReadError>; - - fn next(&mut self) -> Option { - if self.r.eof() { - return None; - } - - let len = match self.r.read_u8() { - Ok(x) => x, - Err(e) => return Some(Err(e)), - }; - let type_ = match self.r.read_u8() { - Ok(x) => x, - Err(e) => return Some(Err(e)), - }; - let data = match self.r.read_slice(len as usize - 2) { - Ok(x) => x, - Err(e) => return Some(Err(e)), - }; - - Some(Ok((type_, Reader::new(data)))) - } -} - -#[derive(Copy, Clone, PartialEq, Eq, Debug)] -#[cfg_attr(feature = "defmt", derive(defmt::Format))] -pub struct EndpointInfo { - pub configuration: u8, - pub interface: InterfaceNumber, - pub interface_alt: u8, - pub ep_address: EndpointAddress, -} - -pub fn foreach_endpoint(data: &[u8], mut f: impl FnMut(EndpointInfo)) -> Result<(), ReadError> { - let mut ep = EndpointInfo { - configuration: 0, - interface: InterfaceNumber(0), - interface_alt: 0, - ep_address: EndpointAddress::from(0), - }; - for res in Reader::new(data).read_descriptors() { - let (kind, mut r) = res?; - match kind { - descriptor_type::CONFIGURATION => { - let _total_length = r.read_u16()?; - let _total_length = r.read_u8()?; - ep.configuration = r.read_u8()?; - } - descriptor_type::INTERFACE => { - ep.interface = InterfaceNumber(r.read_u8()?); - ep.interface_alt = r.read_u8()?; - } - descriptor_type::ENDPOINT => { - ep.ep_address = EndpointAddress::from(r.read_u8()?); - f(ep); - } - _ => {} - } - } - Ok(()) -} diff --git a/embassy-usb/src/fmt.rs b/embassy-usb/src/fmt.rs index 8ca61bc39a..8649760ae3 100644 --- a/embassy-usb/src/fmt.rs +++ b/embassy-usb/src/fmt.rs @@ -6,6 +6,19 @@ use core::fmt::{Debug, Display, LowerHex}; #[cfg(all(feature = "defmt", feature = "log"))] compile_error!("You may not enable both `defmt` and `log` features."); +#[collapse_debuginfo(yes)] +macro_rules! bitflags { + ($($x:tt)*) => { + #[cfg(not(feature = "defmt"))] + ::bitflags::bitflags!( + #[derive(Copy, Clone, Eq, PartialEq, Debug)] + $($x)* + ); + #[cfg(feature = "defmt")] + ::defmt::bitflags!($($x)*); + }; +} + #[collapse_debuginfo(yes)] macro_rules! assert { ($($x:tt)*) => { diff --git a/embassy-usb/src/handlers/hub.rs b/embassy-usb/src/handlers/hub.rs new file mode 100644 index 0000000000..9cdd048cbd --- /dev/null +++ b/embassy-usb/src/handlers/hub.rs @@ -0,0 +1,358 @@ +//! Host driver for USB hubs. +//! +//! It has it's own enumerate implementation to deal with the deferred `bus_reset` and state/speed detection. +//! It requires the usb-driver to implement/support `Interrupt` `ChannelIn` endpoints (which resolves a call to `[ChannelIn::read]`). + +use core::num::NonZeroU8; + +use embassy_time::Timer; +use zerocopy::{FromBytes, Immutable, IntoBytes, KnownLayout}; + +use super::{EnumerationInfo, HandlerEvent, RegisterError, UsbHostHandler}; +use crate::control::Request; +use crate::driver::host::{channel, HostError, RequestType, SetupPacket, UsbChannel, UsbHostDriver}; +use crate::driver::{Direction, EndpointInfo, EndpointType, Speed}; +use crate::host::descriptor::{InterfaceDescriptor, USBDescriptor, DEFAULT_MAX_DESCRIPTOR_SIZE}; +use crate::host::ControlChannelExt; + +pub struct HubHandler { + interrupt_channel: H::Channel, + control_channel: H::Channel, + desc: HubDescriptor, + device_address: u8, + device_lut: [Option; MAX_PORTS], + speed: Speed, +} + +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum HubEvent { + DeviceDetected { port: u8, speed: Speed }, + DeviceRemoved { address: Option, port: u8 }, +} + +impl UsbHostHandler for HubHandler { + type PollEvent = HubEvent; + type Driver = H; + + fn static_spec() -> super::StaticHandlerSpec { + super::StaticHandlerSpec { + device_filter: Some(super::DeviceFilter { + base_class: Some(unsafe { NonZeroU8::new_unchecked(0x09) }), // Hub + sub_class: Some(0x00), + protocol: None, // 00 for FS, otherwise HS or higher + }), + } + } + + async fn try_register(bus: &H, enum_info: &EnumerationInfo) -> Result { + let mut control_channel = bus.alloc_channel::( + enum_info.device_address, + &EndpointInfo::new( + 0.into(), + EndpointType::Control, + enum_info + .device_desc + .max_packet_size0 + .min(if enum_info.ls_over_fs { 8 } else { 64 }) as u16, + ), + enum_info.ls_over_fs, + )?; + + let mut cfg_desc_buf = [0u8; DEFAULT_MAX_DESCRIPTOR_SIZE]; + let configuration = enum_info + .active_config_or_set_default(&mut control_channel, &mut cfg_desc_buf) + .await?; + + let iface = configuration + .iter_interface() + .find(|v| { + matches!( + v, + InterfaceDescriptor { + interface_class: 0x09, + interface_subclass: 0x0, + interface_protocol: 0x0, + .. + } + ) + }) + .ok_or(RegisterError::NoSupportedInterface)?; + + let interrupt_ep = iface + .iter_endpoints() + .find(|v| v.ep_type() == EndpointType::Interrupt && v.ep_dir() == Direction::In) + .ok_or(RegisterError::NoSupportedInterface)?; + + let interrupt_channel = bus.alloc_channel::( + enum_info.device_address, + &interrupt_ep.into(), + enum_info.ls_over_fs, + )?; + + let desc = control_channel.request_descriptor::(0, true).await?; + + let mut hub = HubHandler { + interrupt_channel, + control_channel, + desc, + device_address: enum_info.device_address, + device_lut: [None; MAX_PORTS], + speed: enum_info.speed, + }; + + // Power-On ports + for port in 0..hub.desc.port_num { + hub.port_feature(true, PortFeature::Power, port, 0).await?; + } + + Timer::after_millis(hub.desc.power_on_delay as u64 * 2).await; + + Ok(hub) + } + + async fn wait_for_event(&mut self) -> Result, HostError> { + loop { + // Wait for status change + let mut buf = [0u8; 16]; + // 1 bit per port + 1 reserved + let slice = &mut buf[..(self.desc.port_num as usize / 8) + 1]; + self.interrupt_channel.request_in(slice).await?; + + let mut hub_changes = HubInterrupt(slice); + while let Some(port) = hub_changes.take_port_change() { + trace!( + "HUB {}: port {} is changed, requesting status", + self.device_address, + port + ); + + // Get status + let (status, change) = self.get_port_status(port as u8).await?; + debug!( + "HUB {}: port {} status: {} change: {}", + self.device_address, port, status, change + ); + + // TODO: Overcurrent protection + // Clear reset change after reset + if change.contains(PortStatusChange::RESET) { + self.port_feature(false, PortFeature::ChangeReset, port, 0).await?; + } + + if change.contains(PortStatusChange::CONNECT) { + // Clear connect status change + self.port_feature(false, PortFeature::ChangeConnection, port, 0).await?; + match status.contains(PortStatus::CONNECTED) { + // Device connected, perform bus reset and configure + true => { + // Determine speed + let speed: Speed = status.into(); + + debug!( + "HUB {}: Device connected to port {} with {} speed", + self.device_address, port, speed + ); + + // User can now `enumerate_port` + return Ok(HandlerEvent::HandlerEvent(HubEvent::DeviceDetected { port, speed })); + } + // Device disconnected, remove from registry + false => { + debug!("HUB {}: Device disconnected from port {}", self.device_address, port); + let device_ref = self.device_lut.get_mut(port as usize); + return Ok(HandlerEvent::HandlerEvent(HubEvent::DeviceRemoved { + address: device_ref.and_then(|v| v.take()), + port, + })); + } + } + } + } + } + } +} + +impl HubHandler { + async fn enumerate_port<'a>( + &mut self, + port: u8, + speed: Speed, + new_device_address: u8, + ) -> Result { + // NOTE: we probably could do this in the wait loop but it would require a arc mutex registry which seems unnecessary + self.port_feature(true, PortFeature::Reset, port, 0).await?; + Timer::after_millis(50).await; + self.port_feature(false, PortFeature::ChangeReset, port, 0).await?; + + let ls_pre = matches!((speed, self.speed), (Speed::Low, Speed::Full | Speed::High)); + + self.control_channel + .enumerate_device(speed, new_device_address, ls_pre) + .await + } + + /// Set/Clear PortFeature + /// + /// USB 2.0 Spec: 11.24.2.13,1 + async fn port_feature(&mut self, set: bool, feature: PortFeature, port: u8, selector: u8) -> Result<(), HostError> { + let setup = SetupPacket { + request_type: RequestType::OUT | RequestType::TYPE_CLASS | RequestType::RECIPIENT_OTHER, + request: if set { + Request::SET_FEATURE + } else { + Request::CLEAR_FEATURE + }, + value: feature as u16, + index: ((selector as u16) << 8) | (port + 1) as u16, + length: 0, + }; + + self.control_channel.control_out(&setup, &[]).await?; + Ok(()) + } + + /// GetPortStatus + /// + /// USB 2.0 Spec: 11.24.2.7 + async fn get_port_status(&mut self, port: u8) -> Result<(PortStatus, PortStatusChange), HostError> { + let setup = SetupPacket { + request_type: RequestType::IN | RequestType::TYPE_CLASS | RequestType::RECIPIENT_OTHER, + request: Request::GET_STATUS, + value: 0, + index: (port + 1) as u16, + length: 4, + }; + + let mut buf = [0u16; 2]; + + self.control_channel.control_in(&setup, buf.as_mut_bytes()).await?; + + let status = PortStatus::from_bits_truncate(buf[0]); + let change = PortStatusChange::from_bits_truncate(buf[1]); + + Ok((status, change)) + } +} + +pub struct HubInterrupt<'a>(&'a mut [u8]); + +impl<'a> HubInterrupt<'a> { + fn is_hub_change(&self) -> bool { + // SAFETY: at least one byte is required by construction + (unsafe { self.0.get_unchecked(0) } & 1 != 0) + } + + fn take_port_change(&mut self) -> Option { + self.0 + .iter_mut() + .enumerate() + .find(|(idx, v)| v.trailing_zeros() >= if *idx != 0 { 0 } else { 1 }) + .map(|(idx, v)| { + let bit = v.trailing_zeros() as usize; + *v &= !(1 << bit); + (bit + idx * 8 + 1) as u8 + }) + } +} + +/// USB 2.0 Spec heading: 11.23.2.1 +#[derive(KnownLayout, FromBytes, Immutable, Clone, Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C)] +struct HubDescriptor { + len: u8, + desc_type: u8, + port_num: u8, + characteristics0: u8, + characteristics1: u8, + /// ms x 2 + power_on_delay: u8, + /// Maximum controller current + max_current: u8, + /// 8 + 8 bits per port, at max 127 ports => 32 bytes + port_buf: [u8; 32], +} + +impl USBDescriptor for HubDescriptor { + const SIZE: usize = core::mem::size_of::(); + + const DESC_TYPE: u8 = 0x29; + + type Error = (); + + fn try_from_bytes(bytes: &[u8]) -> Result + where + Self: Sized, + { + let (byref, _) = Self::ref_from_prefix(bytes).map_err(|_| ())?; + if byref.desc_type != Self::DESC_TYPE { + return Err(()); + } + + Ok(byref.clone()) + } +} + +/// USB 2.0 Spec: 11.24.2 Table 11-17 +#[derive(Clone, Copy)] +#[repr(u8)] +enum PortFeature { + Connection = 0, + Enable, + Suspend, + OverCurrent, + Reset, + Power = 8, + LowSpeed, + ChangeConnection = 16, + ChangeEnable, + ChangeSuspend, + ChangeOverCurrent, + ChangeReset, + Test, + Indicator, +} + +bitflags! { + /// USB 2.0 Spec: 11.24.2.7.1 + struct PortStatus: u16 { + const CONNECTED = 1 << 0; + const ENABLED = 1 << 1; + const SUSPENDED = 1 << 2; + const OVERCURRENT = 1 << 3; + const RESET = 1 << 4; + // Reserved: 5..8 + const POWERED = 1 << 8; + const LOW_SPEED = 1 << 9; + const HIGH_SPEED = 1 << 10; + const TEST_MODE = 1 << 11; + const INDICATOR_CUSTOM_COLOR = 1 << 12; + // Reserved: 13..16 + } +} + +bitflags! { + /// USB 2.0 Spec: 11.24.2.7.2 + struct PortStatusChange: u16 { + const CONNECT = 1 << 0; + const ENABLE = 1 << 1; + const SUSPEND = 1 << 2; + const OVERCURRENT = 1 << 3; + const RESET = 1 << 4; + // Reserved: 5..16 + } +} + +impl From for Speed { + fn from(value: PortStatus) -> Self { + let ls = value.contains(PortStatus::LOW_SPEED); + let hs = value.contains(PortStatus::HIGH_SPEED); + + match (ls, hs) { + (true, _) => Speed::Low, + (false, false) => Speed::Full, + (false, true) => Speed::High, + } + } +} diff --git a/embassy-usb/src/handlers/kbd.rs b/embassy-usb/src/handlers/kbd.rs new file mode 100644 index 0000000000..f7bda05c03 --- /dev/null +++ b/embassy-usb/src/handlers/kbd.rs @@ -0,0 +1,207 @@ +//! Host driver for basic keyboard HID inputs + +use core::num::NonZeroU8; + +use super::{EnumerationInfo, HandlerEvent, RegisterError, UsbHostHandler}; +use crate::driver::host::{channel, HostError, UsbChannel, UsbHostDriver}; +use crate::driver::{Direction, EndpointInfo, EndpointType}; +use crate::host::descriptor::{InterfaceDescriptor, USBDescriptor, DEFAULT_MAX_DESCRIPTOR_SIZE}; +use crate::host::ControlChannelExt; + +#[repr(C)] +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct KeyStatusUpdate { + /// Bitfield of modifiers keys: + /// LeftControl + /// LeftShift + /// LeftAlt + /// Left GUI + /// RightControl + /// RightShift + /// RightAlt + /// Right GUI + pub modifiers: u8, + /// Reserved for OEM. + /// Should be ignored. + pub reserved: u8, + /// Keycodes of currently pressed keys. + /// 0 -> not pressed + /// 1 -> rollover + /// See "HID Usage Tables FOR Universal Serial Bus (USB)" for all values. + pub keypress: [Option; 6], +} + +impl KeyStatusUpdate { + fn from_buffer_unchecked(value: [u8; 8]) -> Self { + // SAFETY: Option is None when u8 = 0 + unsafe { core::mem::transmute(value) } + } +} + +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum KbdEvent { + KeyStatusUpdate(KeyStatusUpdate), +} + +/// Host side driver for HID boot keyboard +pub struct KbdHandler { + interrupt_channel: H::Channel, + control_channel: H::Channel, +} + +impl UsbHostHandler for KbdHandler { + type PollEvent = KbdEvent; + type Driver = H; + + fn static_spec() -> super::StaticHandlerSpec { + super::StaticHandlerSpec { device_filter: None } + } + + async fn try_register(bus: &H, enum_info: &EnumerationInfo) -> Result { + let mut control_channel = bus.alloc_channel::( + enum_info.device_address, + &EndpointInfo::new( + 0.into(), + EndpointType::Control, + (enum_info.device_desc.max_packet_size0 as u16).min(enum_info.speed.max_packet_size()), + ), + enum_info.ls_over_fs, + )?; + + let mut cfg_desc_buf = [0u8; DEFAULT_MAX_DESCRIPTOR_SIZE]; + let configuration = enum_info + .active_config_or_set_default(&mut control_channel, &mut cfg_desc_buf) + .await?; + + let iface = configuration + .iter_interface() + .find(|v| { + matches!( + v, + InterfaceDescriptor { + interface_class: 0x03, + interface_subclass: 0x1, + interface_protocol: 0x1, + .. + } + ) + }) + .ok_or(RegisterError::NoSupportedInterface)?; + + let interrupt_ep = iface + .iter_endpoints() + .find(|v| v.ep_type() == EndpointType::Interrupt && v.ep_dir() == Direction::In) + .ok_or(RegisterError::NoSupportedInterface)?; + + configuration.set_configuration(&mut control_channel).await?; + + let interrupt_channel = bus.alloc_channel::( + enum_info.device_address, + &interrupt_ep.into(), + enum_info.ls_over_fs, + )?; + + debug!("[kbd]: Setting PROTOCOL & idle"); + const SET_PROTOCOL: u8 = 0x0B; + const BOOT_PROTOCOL: u16 = 0x0000; + if let Err(err) = control_channel + .class_request_out(SET_PROTOCOL, BOOT_PROTOCOL, iface.interface_number as u16, &[]) + .await + { + error!("[kbd]: Failed to set protocol: {:?}", err); + } + + const SET_IDLE: u8 = 0x0A; + if let Err(err) = control_channel + .class_request_out(SET_IDLE, 0, iface.interface_number as u16, &[]) + .await + { + error!("[kbd]: Failed to set idle: {:?}", err); + } + + Ok(KbdHandler { + interrupt_channel, + control_channel, + }) + } + + async fn wait_for_event(&mut self) -> Result, HostError> { + let mut buffer = [0u8; 8]; + + debug!("[kbd]: Requesting interrupt IN"); + self.interrupt_channel.request_in(&mut buffer[..]).await?; + debug!("[kbd]: Got interrupt {:?}", buffer); + + Ok(HandlerEvent::HandlerEvent(KbdEvent::KeyStatusUpdate( + KeyStatusUpdate::from_buffer_unchecked(buffer), + ))) + } +} + +bitflags! { + /// Command keyboard state options + pub struct KeyboardState: u8 { + /// Enables NumLock + const NUM_LOCK = 1 << 0; + /// Enables CapsLock + const CAPS_LOCK = 1 << 1; + /// Enables ScrollLock + const SCROLL_LOCK = 1 << 2; + /// Enables Compose-mode + const COMPOSE = 1 << 3; + /// Enables Kana-mode + const KANA = 1 << 4; + } +} + +impl KbdHandler { + /// Sets the state of the keyboard + pub async fn set_state(&mut self, state: &KeyboardState) -> Result<(), HostError> { + const SET_REPORT: u8 = 0x09; + const OUTPUT_REPORT: u16 = 2 << 8; + self.control_channel + .class_request_out(SET_REPORT, OUTPUT_REPORT, 0, &[state.bits()]) + .await + } +} + +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct HIDDescriptor { + pub len: u8, + pub descriptor_type: u8, + pub bcd_hid: u16, + pub country_code: u8, + pub num_descriptors: u8, + + // num_descriptors determines how many pairs of descriptor_typeI/descriptor_lengthI follow. + pub descriptor_type0: u8, + pub descriptor_length0: u16, +} + +impl USBDescriptor for HIDDescriptor { + const SIZE: usize = 9; // only valid for 1 descriptor + const DESC_TYPE: u8 = 33; + + type Error = (); + + fn try_from_bytes(bytes: &[u8]) -> Result { + if bytes.len() < Self::SIZE { + return Err(()); + } + if bytes[1] != Self::DESC_TYPE { + return Err(()); + } + + Ok(Self { + len: bytes[0], + descriptor_type: bytes[1], + bcd_hid: u16::from_le_bytes([bytes[2], bytes[3]]), + country_code: bytes[4], + num_descriptors: bytes[5], + descriptor_type0: bytes[6], + descriptor_length0: u16::from_le_bytes([bytes[7], bytes[8]]), + }) + } +} diff --git a/embassy-usb/src/handlers/mod.rs b/embassy-usb/src/handlers/mod.rs new file mode 100644 index 0000000000..f111ff9386 --- /dev/null +++ b/embassy-usb/src/handlers/mod.rs @@ -0,0 +1,213 @@ +//! Implementations of common Host-side drivers + +use core::num::NonZeroU8; + +use crate::driver::host::channel::{self, IsIn, IsOut}; +use crate::driver::host::{HostError, UsbChannel, UsbHostDriver}; +use crate::driver::Speed; +use crate::host::descriptor::{ConfigurationDescriptor, DeviceDescriptor, USBDescriptor}; +use crate::host::ControlChannelExt; + +pub mod hub; +pub mod kbd; + +pub struct DeviceFilter { + /// Device base-class, 0 would mean it's defined on an interface-level + pub base_class: Option, + pub sub_class: Option, + pub protocol: Option, +} + +pub struct StaticHandlerSpec { + /// A non-exhaustive filter for devices; the final filter is done inside try_register + pub device_filter: Option, +} + +impl StaticHandlerSpec { + pub fn new(device_filter: Option) -> Self { + StaticHandlerSpec { device_filter } + } +} + +/// Information obtained through preliminary enumeration, required for further configuration +#[derive(Debug)] +pub struct EnumerationInfo { + /// Device address + pub device_address: u8, + /// Used to indicate a low-speed device over a full-speed or higher interface + pub ls_over_fs: bool, + /// Negotiated speed of the device + pub speed: Speed, + /// Device Specs + pub device_desc: DeviceDescriptor, +} + +impl EnumerationInfo { + /// Retrieves active device configuration or sets the default if not yet configured + /// + /// A USB device can only have a single configuration active, this method ensures any previously + /// configured mode is maintained for interface/endpoint configuration + pub async fn active_config_or_set_default<'a, D: IsIn + IsOut, C: UsbChannel>( + &self, + channel: &mut C, + cfg_desc_buf: &'a mut [u8], + ) -> Result, HostError> { + // FIXME: We can't just call `get_active_configuration`, as of writing this is an unfortunate limitation of the borrow checker (see https://users.rust-lang.org/t/yet-another-returning-this-value-requires-that-x-is-borrowed-for-a/112604/2) + Ok(match channel.active_configuration_value().await? { + Some(_) => self.get_active_configuration(channel, cfg_desc_buf).await?.unwrap(), + None => { + // No active configuration, set default + let default_cfg = self.get_configuration(0, channel, cfg_desc_buf).await?; + default_cfg.set_configuration(channel).await?; + default_cfg + } + }) + } + + /// Retrieves the active device configuration, `None` if currently no configuration is active. + pub async fn get_active_configuration<'a, D: IsIn, C: UsbChannel>( + &self, + channel: &mut C, + cfg_desc_buf: &'a mut [u8], + ) -> Result>, HostError> { + let cfg_id = channel.active_configuration_value().await?; + + let cfg_id = match cfg_id { + Some(v) => v.into(), + None => return Ok(None), + }; + + let mut index = None; + for i in 0..self.device_desc.num_configurations { + let cfg_desc_short = channel + .request_descriptor::(i, false) + .await?; + + if cfg_desc_short.configuration_value == cfg_id { + if cfg_desc_short.total_len as usize > cfg_desc_buf.len() { + return Err(HostError::InsufficientMemory); + } + + index.replace(i); + break; + } + } + + let index = index.ok_or(HostError::Other( + "Active Configuration not found on device, bad device?", + ))?; + + channel + .request_descriptor_bytes(ConfigurationDescriptor::DESC_TYPE, index, cfg_desc_buf) + .await?; + + let cfg_desc = + ConfigurationDescriptor::try_from_slice(cfg_desc_buf).map_err(|_| HostError::InvalidDescriptor)?; + + Ok(Some(cfg_desc)) + } + + /// Retrieve a device configuration by index up to a max of [`DeviceDescriptor::num_configurations`] + pub async fn get_configuration<'a, D: channel::IsIn, C: UsbChannel>( + &self, + index: u8, + channel: &mut C, + cfg_desc_buf: &'a mut [u8], + ) -> Result, HostError> { + if index >= self.device_desc.num_configurations { + return Err(HostError::InvalidDescriptor); + } + + let cfg_desc_short = channel + .request_descriptor::(index, false) + .await?; + + let total_len = cfg_desc_short.total_len as usize; + if total_len > cfg_desc_buf.len() { + return Err(HostError::InsufficientMemory); + } + let dest_buffer = &mut cfg_desc_buf[0..total_len]; + + channel + .request_descriptor_bytes(ConfigurationDescriptor::DESC_TYPE, index, dest_buffer) + .await?; + + trace!( + "Full Configuration Descriptor [{}]: {:?}", + cfg_desc_short.total_len, + dest_buffer + ); + + let cfg_desc = + ConfigurationDescriptor::try_from_slice(dest_buffer).map_err(|_| HostError::InvalidDescriptor)?; + + Ok(cfg_desc) + } +} + +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum HandlerEvent { + NoChange, + HandlerDisconnected, + HandlerEvent(T), +} + +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum RegisterError { + NoSupportedInterface, + InvalidDescriptor, + HostError(HostError), +} + +impl From for RegisterError { + fn from(value: HostError) -> Self { + RegisterError::HostError(value) + } +} + +/// The base functionality for a host-side USB driver +/// +/// In order to speed up driver detection when large amount of drivers may be supported +/// we support a declarative filter with `static_spec`, this allows a user to build a hash-table, b-tree or similar structure +/// which can be traversed efficiently upon enumeration. +pub trait UsbHostHandler: Sized { + type Driver: UsbHostDriver; + type PollEvent; + + /// A static specification of the handler + fn static_spec() -> StaticHandlerSpec; + + /// Attempts to build a driver from a enumerated device + /// It's expected that the device has been given a unique address through the enumeration process + /// + /// Typically this is implemented by retrieving the (interface) descriptors and sifting through those + /// if a valid configuration is found the device is configured using `SET_CONFIGURATION`; + /// finally the appropriate channels are allocated and stored in the resulting handler + /// + /// NOTE: Channels are expected to self-clean on `Drop`. FIXME: this is not the case for stm32 + async fn try_register(bus: &Self::Driver, enum_info: &EnumerationInfo) -> Result; + + /// Wait for changes to handler, the handler is expected to defer (`yield_now` or Timer::after) whenever idle. + /// Handler users should not use `select` or `join` to avoid dropping futures. + /// + /// Events are handler-specific and may provide anything in their `HandlerEvent` + /// drivers that deal with sub-devices (e.g. Hubs) may use [`HandlerEvent::HandlerDisconnected`] to indicate all sub-devices + /// have also disconnected + async fn wait_for_event(&mut self) -> Result, HostError>; +} + +/// An extension to UsbHostHandler allowing the handler to be suspended in order to handle other devices (by dropping channels) +/// +/// This should be implementable for most handlers however depending on the amount of channels +/// used it might not be worth implementing for all handlers. +pub trait UsbResumableHandler: UsbHostHandler { + type UsbResumeInfo; + + /// In theory this doesn't need to be async, but a implementer might desire to run some checks upon resuming + async fn try_resume(bus: &Self::Driver, resume_info: Self::UsbResumeInfo) -> Result; + + // Consumes `Self` to gain resume info, this prevents any duplication + async fn try_suspend(self, bus: &mut Self::Driver) -> Self::UsbResumeInfo; +} diff --git a/embassy-usb/src/host/descriptor.rs b/embassy-usb/src/host/descriptor.rs new file mode 100644 index 0000000000..fcc7fee59d --- /dev/null +++ b/embassy-usb/src/host/descriptor.rs @@ -0,0 +1,621 @@ +use embassy_usb_driver::host::HostError; +use embassy_usb_driver::{Direction, EndpointInfo, EndpointType}; + +use crate::descriptor::descriptor_type; + +pub(crate) const DEFAULT_MAX_DESCRIPTOR_SIZE: usize = 512; +pub type StringIndex = u8; + +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum DescriptorError { + BadDescriptorType, + UnexpectedEndOfBuffer, +} + +/// First 8 bytes of the DeviceDescriptor. This is used to figure out the `max_packet_size0` value to reconfigure channel 0. +/// All USB devices support max_packet_size0=8 which is why the first 8 bytes of the descriptor can always be read. +#[allow(missing_docs)] +#[derive(Debug)] +pub struct DeviceDescriptorPartial { + _padding: [u8; 7], + pub max_packet_size0: u8, +} + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[allow(missing_docs)] +pub struct DeviceDescriptor { + pub len: u8, + pub descriptor_type: u8, + pub bcd_usb: u16, + pub device_class: u8, + pub device_subclass: u8, + pub device_protocol: u8, + pub max_packet_size0: u8, + pub vendor_id: u16, + pub product_id: u16, + pub bcd_device: u16, + pub manufacturer: StringIndex, + pub product: StringIndex, + pub serial_number: StringIndex, + pub num_configurations: u8, +} + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[allow(missing_docs)] +pub struct ConfigurationDescriptor<'a> { + pub len: u8, + pub descriptor_type: u8, + pub total_len: u16, + pub num_interfaces: u8, + pub configuration_value: u8, + pub configuration_name: StringIndex, + pub attributes: u8, + pub max_power: u8, + + pub buffer: &'a [u8], +} + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[allow(missing_docs)] +pub struct InterfaceDescriptor<'a> { + pub len: u8, + pub descriptor_type: u8, + pub interface_number: u8, + pub alternate_setting: u8, + pub num_endpoints: u8, + pub interface_class: u8, + pub interface_subclass: u8, + pub interface_protocol: u8, + pub interface_name: StringIndex, + + /// All additional bytes end up in this buffer. + /// This buffer can then be used to parse endpoint descriptors or class descriptors + pub buffer: &'a [u8], +} + +/// Trait to be implemented by fixed size descriptors for automatic parsing. +pub trait USBDescriptor { + /// Fixed size of the descriptor + /// For varying length descriptors, this cannot be used and they have to be parsed outside of this module. + const SIZE: usize; + + /// The descriptor type that has to match the type of this descriptor. + const DESC_TYPE: u8; + + /// The type returned on error + type Error; + + /// Try to parse the descriptor from a byte slice + fn try_from_bytes(bytes: &[u8]) -> Result + where + Self: Sized; +} + +impl USBDescriptor for DeviceDescriptorPartial { + const SIZE: usize = 8; + + const DESC_TYPE: u8 = descriptor_type::DEVICE; + + type Error = (); + + fn try_from_bytes(bytes: &[u8]) -> Result { + if bytes.len() < Self::SIZE { + return Err(()); + } + if bytes[1] != Self::DESC_TYPE { + return Err(()); + } + Ok(Self { + _padding: [0; 7], + max_packet_size0: bytes[7], + }) + } +} + +impl USBDescriptor for DeviceDescriptor { + const SIZE: usize = 18; + + const DESC_TYPE: u8 = descriptor_type::DEVICE; + type Error = (); + + fn try_from_bytes(bytes: &[u8]) -> Result { + if bytes.len() < Self::SIZE { + return Err(()); + } + if bytes[1] != Self::DESC_TYPE { + return Err(()); + } + Ok(Self { + len: bytes[0], + descriptor_type: bytes[1], + bcd_usb: u16::from_le_bytes([bytes[2], bytes[3]]), + device_class: bytes[4], + device_subclass: bytes[5], + device_protocol: bytes[6], + max_packet_size0: bytes[7], + vendor_id: u16::from_le_bytes([bytes[8], bytes[9]]), + product_id: u16::from_le_bytes([bytes[10], bytes[11]]), + bcd_device: u16::from_le_bytes([bytes[12], bytes[13]]), + manufacturer: bytes[14], + product: bytes[15], + serial_number: bytes[16], + num_configurations: bytes[17], + }) + } +} + +impl USBDescriptor for ConfigurationDescriptor<'_> { + const SIZE: usize = 9; + + const DESC_TYPE: u8 = descriptor_type::CONFIGURATION; + type Error = (); + + fn try_from_bytes(bytes: &[u8]) -> Result { + if bytes.len() < Self::SIZE { + return Err(()); + } + if bytes[1] != Self::DESC_TYPE { + return Err(()); + } + + Ok(Self { + len: bytes[0], + descriptor_type: bytes[1], + total_len: u16::from_le_bytes([bytes[2], bytes[3]]), + num_interfaces: bytes[4], + configuration_value: bytes[5], + configuration_name: bytes[6], + attributes: bytes[7], + max_power: bytes[8], + buffer: &[], + }) + } +} + +/// Iterates over the InterfaceDescriptors of a single configuration +/// Note that, there might be more InterfaceDescriptors than interfaces. +pub struct InterfaceIterator<'a> { + offset: usize, + cfg_desc: &'a ConfigurationDescriptor<'a>, +} + +impl<'a> Iterator for InterfaceIterator<'a> { + type Item = InterfaceDescriptor<'a>; + + fn next(&mut self) -> Option { + if self.offset >= self.cfg_desc.buffer.len() { + None + } else { + // Assuming spec compliant descriptors the next descriptor should always be at our offset + let remaining_buf = &self.cfg_desc.buffer[self.offset..]; + // FIXME: Fallible, propagate errors? + let iface = InterfaceDescriptor::try_from_bytes(remaining_buf).ok()?; + self.offset += iface.len as usize + iface.buffer.len(); + Some(iface) + } + } +} + +impl<'a> ConfigurationDescriptor<'a> { + /// Parses a full Configuration Descriptor with reference to sub-descriptors + pub fn try_from_slice(buf: &'a [u8]) -> Result, HostError> { + if buf.len() < Self::SIZE { + return Err(HostError::InvalidDescriptor); + } + if buf[1] != Self::DESC_TYPE { + return Err(HostError::InvalidDescriptor); + } + + let total_length = u16::from_le_bytes([buf[2], buf[3]]); + Ok(Self { + len: buf[0], + descriptor_type: buf[1], + total_len: total_length, + num_interfaces: buf[4], + configuration_value: buf[5], + configuration_name: buf[6], + attributes: buf[7], + max_power: buf[8], + buffer: &buf[buf[0] as usize..total_length as usize], + }) + } + + /// Iterate over all (raw) descriptors contained in this Configuration + pub fn iter_descriptors(&self) -> RawDescriptorIterator<'_> { + RawDescriptorIterator { + buf: self.buffer, + offset: 0, + } + } + + /// Iterate over all interface descriptors of this Configuration + pub fn iter_interface(&self) -> InterfaceIterator<'_> { + InterfaceIterator { + offset: 0, + cfg_desc: self, + } + } + + fn buffer_sliced(&self) -> &[u8] { + // The configuration descriptor's own bytes are already consumed. + let end = self.total_len as usize - Self::SIZE; + &self.buffer[..end] + } +} + +/// Iterates over raw descriptors (assuming correctly formed), returning the byte offset & buffer +pub struct RawDescriptorIterator<'a> { + buf: &'a [u8], + offset: usize, +} + +impl<'a> Iterator for RawDescriptorIterator<'a> { + type Item = (usize, &'a [u8]); + + fn next(&mut self) -> Option { + if self.offset >= self.buf.len() { + return None; + } + let pre_offset = self.offset; + let len = self.buf[pre_offset] as usize; + self.offset += len; + Some((pre_offset, &self.buf[pre_offset..self.offset])) + } +} + +/// Iterates over the endpoints of an interface +// +/// Equivalent to `RawDescriptorIterator{}.take_while(|v| v[1] != InterfaceDescriptor::DESC_TYPE).filter_map(|v| EndpointDescriptor::try_from_bytes(v).ok())` +pub struct EndpointIterator<'a> { + buffer_idx: usize, + index: usize, + iface_desc: &'a InterfaceDescriptor<'a>, +} + +impl Iterator for EndpointIterator<'_> { + type Item = EndpointDescriptor; + + fn next(&mut self) -> Option { + if self.index >= self.iface_desc.num_endpoints as usize { + None + } else { + // Cannot assume a only standard descriptors https://wiki.osdev.org/Universal_Serial_Bus#Standard_USB_Descriptors:~:text=Therefore%2C%20the%20system%20software,least%20the%20expected%20length. + + while self.buffer_idx + 7 <= self.iface_desc.buffer.len() { + let working_buffer = &self.iface_desc.buffer[self.buffer_idx..]; + self.buffer_idx += working_buffer[0] as usize; + if let Ok(descr) = EndpointDescriptor::try_from_bytes(working_buffer) { + self.index += 1; + return Some(descr); + } + } + None + } + } +} + +/// InterfaceDescriptor does not implement [USBDescriptor] because it has a borrowed buffer. +/// Since we cannot request an interface descriptor from the device by itself it does not strictly need to implement [USBDescriptor]. +impl<'a> InterfaceDescriptor<'a> { + const SIZE: usize = 9; + + const DESC_TYPE: u8 = descriptor_type::INTERFACE; + + fn try_from_bytes(bytes: &'a [u8]) -> Result { + if bytes.len() < Self::SIZE { + return Err(()); + } + if bytes[1] != Self::DESC_TYPE { + return Err(()); + } + + // Interface descriptor contains no container length info, so we'll have to check each endpoint for their length + let endpoints = &bytes[bytes[0] as usize..]; + + let mut raw_desc_iter = RawDescriptorIterator { + buf: endpoints, + offset: 0, + }; + + // Find boundary of this interface (needs to be parsed linearly unfortunately) + let next_iface_index = raw_desc_iter + .find_map(|(index, v)| { + v.get(1) + .is_some_and(|v| *v == InterfaceDescriptor::DESC_TYPE) + .then_some(index) + }) + .unwrap_or(endpoints.len()); + + Ok(Self { + len: bytes[0], + descriptor_type: bytes[1], + interface_number: bytes[2], + alternate_setting: bytes[3], + num_endpoints: bytes[4], + interface_class: bytes[5], + interface_subclass: bytes[6], + interface_protocol: bytes[7], + interface_name: bytes[8], + buffer: &endpoints[..next_iface_index], + }) + } + + /// Iterate over (raw) descriptors in this Interface + pub fn iter_descriptors(&self) -> RawDescriptorIterator<'_> { + RawDescriptorIterator { + buf: self.buffer, + offset: 0, + } + } + + /// Iterate over endpoints + pub fn iter_endpoints(&'a self) -> EndpointIterator<'a> { + EndpointIterator { + index: 0, + buffer_idx: 0, + iface_desc: self, + } + } +} + +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct EndpointDescriptor { + /// Length of this descriptor in bytes. + pub len: u8, + /// Type of this descriptor. Must be 0x05. + pub descriptor_type: u8, + /// Endpoint address. + pub endpoint_address: u8, + /// Attributes of this endpoint. + pub attributes: u8, + /// Maximum packet size. + pub max_packet_size: u16, + /// Polling interval. + pub interval: u8, +} + +impl EndpointDescriptor { + /// Returns the endpoint direction based on the address + pub fn ep_dir(&self) -> Direction { + match self.endpoint_address & 0x80 { + 0x00 => Direction::Out, + 0x80 => Direction::In, + _ => unreachable!(), + } + } + + /// Returns the endpoint type as inferred from the `attributes` field. + pub fn ep_type(&self) -> EndpointType { + match self.attributes & 0x03 { + 0 => EndpointType::Control, + 1 => EndpointType::Isochronous, + 2 => EndpointType::Bulk, + 3 => EndpointType::Interrupt, + _ => unreachable!(), + } + } + + /// Create descriptor for CONTROL endpoint + pub fn control(max_packet_size: u16) -> Self { + Self { + len: 8, + descriptor_type: 0x05, + endpoint_address: 0, + attributes: EndpointType::Control as u8, + max_packet_size, + interval: 0, + } + } +} + +impl From for EndpointInfo { + fn from(value: EndpointDescriptor) -> Self { + EndpointInfo { + addr: value.endpoint_address.into(), + ep_type: value.ep_type(), + max_packet_size: value.max_packet_size, + interval_ms: value.interval, + } + } +} + +impl USBDescriptor for EndpointDescriptor { + const SIZE: usize = 7; + + const DESC_TYPE: u8 = descriptor_type::ENDPOINT; + type Error = DescriptorError; + + fn try_from_bytes(bytes: &[u8]) -> Result { + if bytes.len() < Self::SIZE || bytes.len() < bytes[0] as usize { + return Err(DescriptorError::UnexpectedEndOfBuffer); + } + if bytes[1] != Self::DESC_TYPE { + return Err(DescriptorError::BadDescriptorType); + } + Ok(Self { + len: bytes[0], + descriptor_type: bytes[1], + endpoint_address: bytes[2], + attributes: bytes[3], + max_packet_size: u16::from_le_bytes([bytes[4], bytes[5]]), + interval: bytes[6], + }) + } +} + +#[cfg(test)] +mod test { + use heapless::Vec; + + use super::{ConfigurationDescriptor, USBDescriptor}; + use crate::host::EndpointDescriptor; + + #[test] + fn test_parse_extended_endpoint_descriptor() { + // This configuration descriptor has 2 HID interfaces with HID descriptors + // The first endpoint descriptor is extended with 2 bytes such as seen in the MIDI 2.0 + // bRefresh, bSynchAddress (those two bytes are set to 99 in the test bytes below to make them easy to identify). + let desc_bytes = [ + 9, 2, 68, 0, 2, 1, 0, 160, 101, // Configuration descriptor + 9, 4, 0, 0, 1, 3, 1, 1, 0, // Interface 0 + 9, 33, 16, 1, 0, 1, 34, 63, 0, // HID Descriptor + 9, 5, 129, 3, 8, 0, 1, 99, 99, // Endpoint 1 (extended for MIDI 2.0) + 9, 4, 1, 0, 2, 3, 1, 0, 0, // Interface 1 + 9, 33, 16, 1, 0, 1, 34, 39, 0, // HID Descriptor + 7, 5, 131, 3, 64, 0, 1, // Endpoint 1 + 7, 5, 3, 3, 64, 0, 1, // Endpoint 2 + ]; + + let cfg = ConfigurationDescriptor::try_from_slice(desc_bytes.as_slice()).unwrap(); + assert_eq!(cfg.num_interfaces, 2); + + println!("{:?}", cfg.buffer); + + let interface0 = cfg.iter_interface().next().unwrap(); + assert_eq!(interface0.interface_number, 0); + + assert_eq!(interface0.num_endpoints, 1); + + let endpoints: Vec = interface0.iter_endpoints().collect(); + assert_eq!(endpoints.len(), 1); + + let ep = endpoints[0]; + assert_eq!(ep.endpoint_address, 0x81); + assert_eq!(ep.max_packet_size, 8); + + let interface1 = cfg.iter_interface().nth(1).unwrap(); + assert_eq!(interface1.interface_number, 1); + assert_eq!(interface1.num_endpoints, 2); + + let endpoints: Vec = interface1.iter_endpoints().collect(); + assert_eq!(endpoints.len(), 2); + } + + #[test] + fn test_parse_interface_descriptor() { + // This configuration descriptor has 2 HID interfaces with HID descriptors + let desc_bytes = [ + 9, 2, 66, 0, 2, 1, 0, 160, 101, 9, 4, 0, 0, 1, 3, 1, 1, 0, 9, 33, 16, 1, 0, 1, 34, 63, 0, 7, 5, 129, 3, 8, + 0, 1, 9, 4, 1, 0, 2, 3, 1, 0, 0, 9, 33, 16, 1, 0, 1, 34, 39, 0, 7, 5, 131, 3, 64, 0, 1, 7, 5, 3, 3, 64, 0, + 1, + ]; + + let cfg = ConfigurationDescriptor::try_from_slice(desc_bytes.as_slice()).unwrap(); + assert_eq!(cfg.num_interfaces, 2); + // assert!(cfg.buffer_sliced().len() > 16); + + let interface0 = cfg.iter_interface().next().unwrap(); + assert_eq!(interface0.interface_number, 0); + + let interface0_buffer_ref = [9u8, 33, 16, 1, 0, 1, 34, 63, 0, 7, 5, 129, 3, 8, 0, 1]; + assert_eq!(interface0.buffer.len(), interface0_buffer_ref.len()); + + let interface1 = cfg.iter_interface().nth(1).unwrap(); + assert_eq!(interface1.interface_number, 1); + + let interface1_buffer_ref = [ + 9u8, 33, 16, 1, 0, 1, 34, 39, 0, 7, 5, 131, 3, 64, 0, 1, 7, 5, 3, 3, 64, 0, 1, + ]; + assert_eq!(interface1.buffer.len(), interface1_buffer_ref.len()); + } + + #[test] + fn test_parse_endpoint_descriptor() { + // This configuration descriptor has 2 HID interfaces with HID descriptors + let desc_bytes = [ + 9, 2, 66, 0, 2, 1, 0, 160, 101, 9, 4, 0, 0, 1, 3, 1, 1, 0, 9, 33, 16, 1, 0, 1, 34, 63, 0, 7, 5, 129, 3, 8, + 0, 1, 9, 4, 1, 0, 2, 3, 1, 0, 0, 9, 33, 16, 1, 0, 1, 34, 39, 0, 7, 5, 131, 3, 64, 0, 1, 7, 5, 3, 3, 64, 0, + 1, + ]; + + let cfg = ConfigurationDescriptor::try_from_slice(desc_bytes.as_slice()).unwrap(); + assert_eq!(cfg.num_interfaces, 2); + + let interface0 = cfg.iter_interface().next().unwrap(); + assert_eq!(interface0.interface_number, 0); + + assert_eq!(interface0.num_endpoints, 1); + + let endpoints: Vec = interface0.iter_endpoints().collect(); + assert_eq!(endpoints.len(), 1); + + let ep = endpoints[0]; + assert_eq!(ep.endpoint_address, 0x81); + assert_eq!(ep.max_packet_size, 8); + + let interface1 = cfg.iter_interface().nth(1).unwrap(); + assert_eq!(interface1.interface_number, 1); + assert_eq!(interface1.num_endpoints, 2); + + let endpoints: Vec = interface1.iter_endpoints().collect(); + assert_eq!(endpoints.len(), 2); + } + + #[test] + fn test_parse_custom_descriptor() { + // Define a custom descriptor (HID descriptor in this case) + struct HIDDescriptor { + len: u8, + descriptor_type: u8, + bcd_hid: u16, + country_code: u8, + num_descriptors: u8, + descriptor_type0: u8, + descriptor_length0: u16, + } + + impl USBDescriptor for HIDDescriptor { + const SIZE: usize = 9; // only valid for 1 descriptor + const DESC_TYPE: u8 = 33; + type Error = (); + fn try_from_bytes(bytes: &[u8]) -> Result { + if bytes.len() < Self::SIZE { + return Err(()); + } + if bytes[1] != Self::DESC_TYPE { + return Err(()); + } + Ok(Self { + len: bytes[0], + descriptor_type: bytes[1], + bcd_hid: u16::from_le_bytes([bytes[2], bytes[3]]), + country_code: bytes[4], + num_descriptors: bytes[5], + descriptor_type0: bytes[6], + descriptor_length0: u16::from_le_bytes([bytes[7], bytes[8]]), + }) + } + } + // This configuration descriptor has 2 HID interfaces with HID descriptors + let desc_bytes = [ + 9, 2, 66, 0, 2, 1, 0, 160, 101, 9, 4, 0, 0, 1, 3, 1, 1, 0, 9, 33, 16, 1, 0, 1, 34, 63, 0, 7, 5, 129, 3, 8, + 0, 1, 9, 4, 1, 0, 2, 3, 1, 0, 0, 9, 33, 16, 1, 0, 1, 34, 39, 0, 7, 5, 131, 3, 64, 0, 1, 7, 5, 3, 3, 64, 0, + 1, + ]; + + let cfg = ConfigurationDescriptor::try_from_slice(desc_bytes.as_slice()).unwrap(); + assert_eq!(cfg.num_interfaces, 2); + + let interface0 = cfg.iter_interface().next().unwrap(); + assert_eq!(interface0.interface_number, 0); + + let hid_desc: HIDDescriptor = interface0 + .iter_descriptors() + .find_map(|v| HIDDescriptor::try_from_bytes(v.1).ok()) + .unwrap(); + + assert_eq!(hid_desc.len, 9); + assert_eq!(hid_desc.descriptor_type, 33); + + assert_eq!(hid_desc.bcd_hid, 0x0110); + assert_eq!(hid_desc.country_code, 0); + assert_eq!(hid_desc.num_descriptors, 1); + assert_eq!(hid_desc.descriptor_type0, 34); + assert_eq!(hid_desc.descriptor_length0, 63); + } +} diff --git a/embassy-usb/src/host/mod.rs b/embassy-usb/src/host/mod.rs new file mode 100644 index 0000000000..536e690f7c --- /dev/null +++ b/embassy-usb/src/host/mod.rs @@ -0,0 +1,316 @@ +//! USB Host implementation +//! +//! Requires an [UsbHostDriver] implementation. +//! + +#![allow(async_fn_in_trait)] + +use core::num::NonZeroU8; + +use embassy_time::Timer; +use embassy_usb_driver::host::{channel, ChannelError, HostError, RequestType, SetupPacket, UsbChannel, UsbHostDriver}; +use embassy_usb_driver::{EndpointInfo, EndpointType, Speed}; + +use crate::control::Request; +use crate::handlers::EnumerationInfo; + +pub mod descriptor; + +use descriptor::*; + +impl ConfigurationDescriptor<'_> { + pub async fn set_configuration>( + &self, + channel: &mut C, + ) -> Result<(), HostError> { + channel.set_configuration(self.configuration_value).await + } +} + +/// Extension trait with convenience methods for control channels +pub trait ControlChannelExt: UsbChannel { + // CONTROL IN methods + /// Request and try to parse the device descriptor. + async fn request_descriptor( + &mut self, + index: u8, + class: bool, + ) -> Result + where + D: channel::IsIn, + { + let mut buf = [0u8; SIZE]; + + // The wValue field specifies the descriptor type in the high byte + // and the descriptor index in the low byte. + let value = ((T::DESC_TYPE as u16) << 8) | index as u16; + + let ty = if class { + RequestType::TYPE_CLASS + } else { + RequestType::TYPE_STANDARD + }; + + let packet = SetupPacket { + request_type: RequestType::IN | ty | RequestType::RECIPIENT_DEVICE, + request: Request::GET_DESCRIPTOR, + value, // descriptor type & index + index: 0, // zero or language ID + length: SIZE as u16, // descriptor length + }; + + self.control_in(&packet, &mut buf).await?; + trace!("Descriptor {}: {=[u8]}", core::any::type_name::(), buf); + + T::try_from_bytes(&buf).map_err(|e| { + // TODO: Log error or make descriptor error not generic + // error!("Device [{}]: Descriptor parse failed: {}", addr, e); + HostError::InvalidDescriptor + }) + } + + /// Request the underlying bytes for a descriptor of a specific type. + /// bytes.len() determines how many bytes are read at maximum. + /// This can be used for descriptors of varying length, which are parsed by the caller. + async fn request_descriptor_bytes(&mut self, desc_type: u8, index: u8, buf: &mut [u8]) -> Result + where + D: channel::IsIn, + { + // The wValue field specifies the descriptor type in the high byte + // and the descriptor index in the low byte. + let value = ((desc_type as u16) << 8) | index as u16; + + let packet = SetupPacket { + request_type: RequestType::IN | RequestType::TYPE_STANDARD | RequestType::RECIPIENT_DEVICE, + request: Request::GET_DESCRIPTOR, + value, // descriptor type & index + index: 0, // zero or language ID + length: buf.len() as u16, // descriptor length + }; + + let len = self.control_in(&packet, buf).await?; + Ok(len) + } + + /// Request the underlying bytes for an additional descriptor of a specific interface. + /// Useful for class specific descriptors of varying length. + /// bytes.len() determines how many bytes are read at maximum. + async fn interface_request_descriptor_bytes( + &mut self, + interface_num: u8, + buf: &mut [u8], + ) -> Result + where + D: channel::IsIn, + { + // The wValue field specifies the descriptor type in the high byte + // and the descriptor index in the low byte. + let value = (T::DESC_TYPE as u16) << 8; + + let packet = SetupPacket { + request_type: RequestType::IN | RequestType::TYPE_STANDARD | RequestType::RECIPIENT_INTERFACE, + request: Request::GET_DESCRIPTOR, + value, // descriptor type & index + index: interface_num as u16, // zero or language ID + length: buf.len() as u16, // descriptor length + }; + + let len = self.control_in(&packet, buf).await?; + Ok(len) + } + + /// GET_CONFIGURATION control request. + /// Retrieves the configurationValue of the active configuration + async fn active_configuration_value(&mut self) -> Result, HostError> + where + D: channel::IsIn, + { + let packet = SetupPacket { + request_type: RequestType::IN | RequestType::TYPE_STANDARD | RequestType::RECIPIENT_DEVICE, + request: Request::GET_CONFIGURATION, + value: 0, + index: 0, + length: 1, + }; + + let mut config_buf = [0u8; 1]; + + self.control_in(&packet, &mut config_buf).await?; + + Ok(NonZeroU8::new(config_buf[0])) + } + + // CONTROL OUT methods + + /// SET_CONFIGURATION control request. + /// Selects the configuration with the given index `config_no`. + async fn set_configuration(&mut self, config_no: u8) -> Result<(), HostError> + where + D: channel::IsOut, + { + let packet = SetupPacket { + request_type: RequestType::OUT | RequestType::TYPE_STANDARD | RequestType::RECIPIENT_DEVICE, + request: Request::SET_CONFIGURATION, + value: config_no as u16, + index: 0, + length: 0, + }; + + self.control_out(&packet, &[]).await?; + + Ok(()) + } + + /// Execute the SET_ADDRESS control request. Assign the given address to the device. + /// Usually done during enumeration. + /// + /// # WARNING + /// This method can break host assumptions. Please do not use it manually + async fn device_set_address(&mut self, new_addr: u8) -> Result<(), HostError> + where + D: channel::IsOut, + { + let packet = SetupPacket { + request_type: RequestType::OUT | RequestType::TYPE_STANDARD | RequestType::RECIPIENT_DEVICE, + request: Request::SET_ADDRESS, + value: new_addr as u16, + index: 0, + length: 0, + }; + + self.control_out(&packet, &[]).await?; + + Ok(()) + } + + /// Execute a control request with request type Class and recipient Interface + async fn class_request_out(&mut self, request: u8, value: u16, index: u16, buf: &[u8]) -> Result<(), HostError> + where + D: channel::IsOut, + { + let packet = SetupPacket { + request_type: RequestType::OUT | RequestType::TYPE_CLASS | RequestType::RECIPIENT_INTERFACE, + request, + value, + index, + length: buf.len() as u16, + }; + + self.control_out(&packet, buf).await?; + + Ok(()) + } + + /// Enumerate *the* currently pending device + /// the device is expected to be reset right before this + /// + /// - `speed` is generally provided by the hardware (core or hub) + /// - `new_device_address` is generated by the software from any available + /// - `ls_over_fs` is derived from the topology (tracker in software), if the device is LS but is plugged into an FS/HS hub it needs this flag set + async fn enumerate_device<'a>( + &mut self, + speed: Speed, + new_device_address: u8, + ls_over_fs: bool, + ) -> Result + where + D: channel::IsIn + channel::IsOut, + { + self.retarget_channel( + 0, + &EndpointInfo::new(0.into(), EndpointType::Control, speed.max_packet_size()), + ls_over_fs, + )?; + + trace!("[enum] Attempting to get max_packet_size for new device"); + let max_packet_size0 = { + let mut max_retries = 10; + loop { + match self + .request_descriptor::(0, false) + .await + { + Ok(desc) => break desc.max_packet_size0, + Err(e) => { + warn!("Request descriptor error: {}, retries: {}", e, max_retries); + if max_retries > 0 { + max_retries -= 1; + Timer::after_millis(1).await; + trace!("Retry Device Descriptor"); + continue; + } else { + return Err(HostError::RequestFailed); + } + } + } + } + }; + + trace!( + "[enum] got max packet size for new device {}, attempting to set address", + max_packet_size0 + ); + + self.device_set_address(new_device_address).await?; + + trace!("[enum] Finished setting address"); + self.retarget_channel( + new_device_address, + &EndpointInfo::new(0.into(), EndpointType::Control, max_packet_size0 as u16), + ls_over_fs, + )?; + + // device has 2ms to change internally by spec but may be faster, we can retry to speed up enumeration + + // TODO: macro this shit + // Retries a request `retries` times until a non-timeout (NAK) is received, if all requests time-out `Err(ChannelError::Timeout)` is returned + let retries = 5; + let device_desc = async { + for _ in 0..retries { + match self + .request_descriptor::(0, false) + .await + { + Err(HostError::ChannelError(ChannelError::Timeout)) => { + Timer::after_millis(1).await; + continue; + } + v => return v, + } + } + Err(HostError::ChannelError(ChannelError::Timeout)) + } + .await?; + + trace!("Device Descriptor: {:?}", device_desc); + + Ok(EnumerationInfo { + device_address: new_device_address, + ls_over_fs, + speed, + device_desc, + }) + } +} + +impl ControlChannelExt for C where C: UsbChannel {} + +/// Extensions for the UsbHostDriver trait +pub trait UsbHostBusExt: UsbHostDriver { + /// Enumerates the root port of the device, without selecting a configuration + async fn enumerate_root_bare( + &mut self, + speed: Speed, + new_device_address: u8, + ) -> Result { + let mut channel = self.alloc_channel::( + 0, + &EndpointInfo::new(0.into(), EndpointType::Control, speed.max_packet_size()), + false, + )?; + + channel.enumerate_device(speed, new_device_address, false).await + } +} + +impl UsbHostBusExt for C {} diff --git a/embassy-usb/src/lib.rs b/embassy-usb/src/lib.rs index 0638fd0a24..e9bbefac61 100644 --- a/embassy-usb/src/lib.rs +++ b/embassy-usb/src/lib.rs @@ -2,6 +2,10 @@ #![doc = include_str!("../README.md")] #![warn(missing_docs)] +#[cfg(test)] +#[macro_use] +extern crate std; + // This mod MUST go first, so that the others see its macros. pub(crate) mod fmt; @@ -11,10 +15,12 @@ mod builder; pub mod class; pub mod control; pub mod descriptor; -mod descriptor_reader; +pub mod handlers; pub mod msos; pub mod types; +pub mod host; + mod config { #![allow(unused)] include!(concat!(env!("OUT_DIR"), "/config.rs")); @@ -22,12 +28,12 @@ mod config { use embassy_futures::select::{select, Either}; use heapless::Vec; +use host::descriptor::ConfigurationDescriptor; pub use crate::builder::{Builder, Config, FunctionBuilder, InterfaceAltBuilder, InterfaceBuilder, UsbVersion}; use crate::config::{MAX_HANDLER_COUNT, MAX_INTERFACE_COUNT}; use crate::control::{InResponse, OutResponse, Recipient, Request, RequestType}; use crate::descriptor::{descriptor_type, lang_id}; -use crate::descriptor_reader::foreach_endpoint; use crate::driver::{Bus, ControlPipe, Direction, Driver, EndpointAddress, Event}; use crate::types::{InterfaceNumber, StringIndex}; @@ -531,12 +537,16 @@ impl<'d, D: Driver<'d>> Inner<'d, D> { self.device_state = UsbDeviceState::Configured; // Enable all endpoints of selected alt settings. - foreach_endpoint(self.config_descriptor, |ep| { - let iface = &self.interfaces[ep.interface.0 as usize]; - self.bus - .endpoint_set_enabled(ep.ep_address, iface.current_alt_setting == ep.interface_alt); - }) - .unwrap(); + let cfg_desc = ConfigurationDescriptor::try_from_slice(self.config_descriptor).unwrap(); + cfg_desc.iter_interface().for_each(|iface| { + iface.iter_endpoints().for_each(|ep| { + let current_iface = &self.interfaces[iface.interface_number as usize]; + self.bus.endpoint_set_enabled( + ep.endpoint_address.into(), + current_iface.current_alt_setting == iface.alternate_setting, + ); + }) + }); // Notify handlers. for h in &mut self.handlers { @@ -551,10 +561,12 @@ impl<'d, D: Driver<'d>> Inner<'d, D> { self.device_state = UsbDeviceState::Addressed; // Disable all endpoints. - foreach_endpoint(self.config_descriptor, |ep| { - self.bus.endpoint_set_enabled(ep.ep_address, false); - }) - .unwrap(); + let cfg_desc = ConfigurationDescriptor::try_from_slice(self.config_descriptor).unwrap(); + cfg_desc.iter_interface().for_each(|iface| { + iface.iter_endpoints().for_each(|ep| { + self.bus.endpoint_set_enabled(ep.endpoint_address.into(), false); + }) + }); // Notify handlers. for h in &mut self.handlers { @@ -583,13 +595,17 @@ impl<'d, D: Driver<'d>> Inner<'d, D> { iface.current_alt_setting = new_altsetting; // Enable/disable EPs of this interface as needed. - foreach_endpoint(self.config_descriptor, |ep| { - if ep.interface == iface_num { - self.bus - .endpoint_set_enabled(ep.ep_address, iface.current_alt_setting == ep.interface_alt); - } - }) - .unwrap(); + let cfg_desc = ConfigurationDescriptor::try_from_slice(self.config_descriptor).unwrap(); + cfg_desc.iter_interface().for_each(|niface| { + niface.iter_endpoints().for_each(|ep| { + if niface.interface_number == iface_num.0 { + self.bus.endpoint_set_enabled( + ep.endpoint_address.into(), + iface.current_alt_setting == niface.alternate_setting, + ); + } + }) + }); // TODO check it is valid (not out of range) diff --git a/examples/rp/Cargo.toml b/examples/rp/Cargo.toml index c8a132a5e8..5b2e7960ef 100644 --- a/examples/rp/Cargo.toml +++ b/examples/rp/Cargo.toml @@ -16,7 +16,7 @@ embassy-net = { version = "0.7.0", path = "../../embassy-net", features = ["defm embassy-net-wiznet = { version = "0.2.0", path = "../../embassy-net-wiznet", features = ["defmt"] } embassy-futures = { version = "0.1.0", path = "../../embassy-futures" } embassy-usb-logger = { version = "0.4.0", path = "../../embassy-usb-logger" } -cyw43 = { version = "0.3.0", path = "../../cyw43", features = ["defmt", "firmware-logs"] } +cyw43 = { version = "0.3.0", path = "../../cyw43", features = ["defmt", "firmware-logs", "bluetooth"] } cyw43-pio = { version = "0.4.0", path = "../../cyw43-pio", features = ["defmt"] } defmt = "1.0.1" diff --git a/examples/rp/src/bin/pio_uart.rs b/examples/rp/src/bin/pio_uart.rs index 485c65204a..b607c91333 100644 --- a/examples/rp/src/bin/pio_uart.rs +++ b/examples/rp/src/bin/pio_uart.rs @@ -16,7 +16,7 @@ use embassy_futures::join::{join, join3}; use embassy_rp::peripherals::{PIO0, USB}; use embassy_rp::pio_programs::uart::{PioUartRx, PioUartRxProgram, PioUartTx, PioUartTxProgram}; use embassy_rp::usb::{Driver, Instance, InterruptHandler}; -use embassy_rp::{bind_interrupts, pio}; +use embassy_rp::{bind_interrupts, pio, PeripheralType}; use embassy_sync::blocking_mutex::raw::NoopRawMutex; use embassy_sync::pipe::Pipe; use embassy_usb::class::cdc_acm::{CdcAcmClass, Receiver, Sender, State}; @@ -133,7 +133,7 @@ impl From for Disconnected { } /// Read from the USB and write it to the UART TX pipe -async fn usb_read<'d, T: Instance + 'd>( +async fn usb_read<'d, T: Instance + PeripheralType + 'd>( usb_rx: &mut Receiver<'d, Driver<'d, T>>, uart_pipe_writer: &mut embassy_sync::pipe::Writer<'_, NoopRawMutex, 20>, ) -> Result<(), Disconnected> { @@ -147,7 +147,7 @@ async fn usb_read<'d, T: Instance + 'd>( } /// Read from the USB TX pipe and write it to the USB -async fn usb_write<'d, T: Instance + 'd>( +async fn usb_write<'d, T: Instance + PeripheralType + 'd>( usb_tx: &mut Sender<'d, Driver<'d, T>>, usb_pipe_reader: &mut embassy_sync::pipe::Reader<'_, NoopRawMutex, 20>, ) -> Result<(), Disconnected> { diff --git a/examples/rp/src/bin/usb_host_keyboard.rs b/examples/rp/src/bin/usb_host_keyboard.rs new file mode 100644 index 0000000000..d8ee61d23e --- /dev/null +++ b/examples/rp/src/bin/usb_host_keyboard.rs @@ -0,0 +1,47 @@ +#![no_std] +#![no_main] + +use defmt::*; +use embassy_executor::Spawner; +use embassy_rp::bind_interrupts; +use embassy_rp::peripherals::USB; +use embassy_usb::driver::host::DeviceEvent::Connected; +use embassy_usb::driver::host::UsbHostDriver; +use embassy_usb::handlers::kbd::KbdHandler; +use embassy_usb::handlers::UsbHostHandler; +use embassy_usb::host::UsbHostBusExt; +use {defmt_rtt as _, panic_probe as _}; + +bind_interrupts!(struct Irqs { + USBCTRL_IRQ => embassy_rp::usb::host::InterruptHandler; +}); + +#[embassy_executor::main] +async fn main(_spawner: Spawner) { + // Initialize Peripherals + let p = embassy_rp::init(Default::default()); + + // Create the driver, from the HAL. + let mut usbhost = embassy_rp::usb::host::Driver::new(*p.USB, Irqs); + + debug!("Detecting device"); + // Wait for root-port to detect device + let speed = loop { + match usbhost.wait_for_device_event().await { + Connected(speed) => break speed, + _ => {} + } + }; + + println!("Found device with speed = {:?}", speed); + + let enum_info = usbhost.enumerate_root_bare(speed, 1).await.unwrap(); + let mut kbd = KbdHandler::try_register(&usbhost, &enum_info) + .await + .expect("Couldn't register keyboard"); + + loop { + let result = kbd.wait_for_event().await; + debug!("{}", result); + } +} diff --git a/examples/rp/src/bin/usb_midi.rs b/examples/rp/src/bin/usb_midi.rs index 3b7910f8bc..9d7c5d9199 100644 --- a/examples/rp/src/bin/usb_midi.rs +++ b/examples/rp/src/bin/usb_midi.rs @@ -8,9 +8,9 @@ use defmt::{info, panic}; use embassy_executor::Spawner; use embassy_futures::join::join; -use embassy_rp::bind_interrupts; use embassy_rp::peripherals::USB; use embassy_rp::usb::{Driver, Instance, InterruptHandler}; +use embassy_rp::{bind_interrupts, PeripheralType}; use embassy_usb::class::midi::MidiClass; use embassy_usb::driver::EndpointError; use embassy_usb::{Builder, Config}; @@ -90,7 +90,9 @@ impl From for Disconnected { } } -async fn midi_echo<'d, T: Instance + 'd>(class: &mut MidiClass<'d, Driver<'d, T>>) -> Result<(), Disconnected> { +async fn midi_echo<'d, T: Instance + PeripheralType + 'd>( + class: &mut MidiClass<'d, Driver<'d, T>>, +) -> Result<(), Disconnected> { let mut buf = [0; 64]; loop { let n = class.read_packet(&mut buf).await?; diff --git a/examples/rp/src/bin/usb_serial.rs b/examples/rp/src/bin/usb_serial.rs index 5e3f0f378a..a8a76b9378 100644 --- a/examples/rp/src/bin/usb_serial.rs +++ b/examples/rp/src/bin/usb_serial.rs @@ -7,9 +7,9 @@ use defmt::{info, panic, unwrap}; use embassy_executor::Spawner; -use embassy_rp::bind_interrupts; use embassy_rp::peripherals::USB; use embassy_rp::usb::{Driver, Instance, InterruptHandler}; +use embassy_rp::{bind_interrupts, PeripheralType}; use embassy_usb::class::cdc_acm::{CdcAcmClass, State}; use embassy_usb::driver::EndpointError; use embassy_usb::UsbDevice; @@ -99,7 +99,9 @@ impl From for Disconnected { } } -async fn echo<'d, T: Instance + 'd>(class: &mut CdcAcmClass<'d, Driver<'d, T>>) -> Result<(), Disconnected> { +async fn echo<'d, T: Instance + PeripheralType + 'd>( + class: &mut CdcAcmClass<'d, Driver<'d, T>>, +) -> Result<(), Disconnected> { let mut buf = [0; 64]; loop { let n = class.read_packet(&mut buf).await?; diff --git a/examples/rp/src/bin/usb_serial_with_logger.rs b/examples/rp/src/bin/usb_serial_with_logger.rs index ea13a1e27d..6507c48474 100644 --- a/examples/rp/src/bin/usb_serial_with_logger.rs +++ b/examples/rp/src/bin/usb_serial_with_logger.rs @@ -8,9 +8,9 @@ use defmt::{info, panic}; use embassy_executor::Spawner; use embassy_futures::join::join; -use embassy_rp::bind_interrupts; use embassy_rp::peripherals::USB; use embassy_rp::usb::{Driver, Instance, InterruptHandler}; +use embassy_rp::{bind_interrupts, PeripheralType}; use embassy_usb::class::cdc_acm::{CdcAcmClass, State}; use embassy_usb::driver::EndpointError; use embassy_usb::{Builder, Config}; @@ -97,7 +97,9 @@ impl From for Disconnected { } } -async fn echo<'d, T: Instance + 'd>(class: &mut CdcAcmClass<'d, Driver<'d, T>>) -> Result<(), Disconnected> { +async fn echo<'d, T: Instance + PeripheralType + 'd>( + class: &mut CdcAcmClass<'d, Driver<'d, T>>, +) -> Result<(), Disconnected> { let mut buf = [0; 64]; loop { let n = class.read_packet(&mut buf).await?; diff --git a/examples/stm32g0/Cargo.toml b/examples/stm32g0/Cargo.toml index bf1e7250e8..41efac634d 100644 --- a/examples/stm32g0/Cargo.toml +++ b/examples/stm32g0/Cargo.toml @@ -12,6 +12,7 @@ embassy-executor = { version = "0.7.0", path = "../../embassy-executor", feature embassy-time = { version = "0.4.0", path = "../../embassy-time", features = ["defmt", "defmt-timestamp-uptime", "tick-hz-32_768"] } embassy-usb = { version = "0.4.0", path = "../../embassy-usb", default-features = false, features = ["defmt"] } embassy-futures = { version = "0.1.0", path = "../../embassy-futures" } +embassy-usb-driver = { version = "0.1.0", path = "../../embassy-usb-driver" } defmt = "1.0.1" defmt-rtt = "1.0.0" diff --git a/examples/stm32g0/src/bin/usb_host_keyboard.rs b/examples/stm32g0/src/bin/usb_host_keyboard.rs new file mode 100644 index 0000000000..32a81c510b --- /dev/null +++ b/examples/stm32g0/src/bin/usb_host_keyboard.rs @@ -0,0 +1,142 @@ +#![no_std] +#![no_main] + +use defmt::*; +use embassy_executor::Spawner; +use embassy_stm32::gpio::{AfType, Level, Output, OutputType, Speed}; +use embassy_stm32::i2c::{self, I2c}; +use embassy_stm32::time::{mhz, Hertz}; +use embassy_stm32::usb::UsbHost; +use embassy_stm32::{bind_interrupts, pac, peripherals, usb, Config}; +use embassy_time::Timer; +use embassy_usb::driver::host::DeviceEvent::Connected; +use embassy_usb::driver::host::UsbHostDriver; +use embassy_usb::handlers::kbd::KbdHandler; +use embassy_usb::handlers::UsbHostHandler; +use embassy_usb::host::UsbHostBusExt; +use {defmt_rtt as _, panic_probe as _}; + +pub use crate::pac::rcc::vals::Mcosel; + +bind_interrupts!(struct Irqs { + USB_UCPD1_2 => usb::USBHostInterruptHandler; + I2C1 => i2c::EventInterruptHandler, i2c::ErrorInterruptHandler; +}); + +#[embassy_executor::main] +async fn main(_spawner: Spawner) { + let mut config = Config::default(); + { + use embassy_stm32::rcc::*; + config.rcc.hse = Some(Hse { + freq: mhz(8), + mode: HseMode::Bypass, + }); + + config.rcc.pll = Some( + // fVCO = fPLLIN × (N / M) = 8 MHz × (16 / 1) = 128 MHz + // • fPLLP = fVCO / P + // • fPLLQ = fVCO / Q + // • fPLLR = fVCO / R + // N = mul + // M = prediv + // PLLRCLK => system clock + // PLLQCLK => USB + // PLLPCLK => unused + // Maximum VCO frequency is 344 MHz. For Range 1 (default) + // 2.66 < PLL / M < 16 + // M = 2 => 8 / 2 = 4 + // N = 30 => 4 * 30 = 120 + // fVCO = 8Mhz / 2 * 60 = 240MHz + // PLLR = 240MHz / 4 = 60MHz + // PLLQ = 240MHz / 5 = 48MHz + Pll { + source: PllSource::HSE, // 8 Mhz + prediv: PllPreDiv::DIV2, + mul: PllMul::MUL60, + divp: None, + divq: Some(PllQDiv::DIV5), // PLLQ should be 48MHz + divr: Some(PllRDiv::DIV4), + }, + ); + config.rcc.sys = Sysclk::PLL1_R; + config.rcc.hsi48 = Some(Hsi48Config { sync_from_usb: true }); + config.rcc.mux.usbsel = mux::Usbsel::PLL1_Q; + } + + let p = embassy_stm32::init(config); + + // configure clock out + pac::RCC + .cfgr() + .modify(|w: &mut pac::rcc::regs::Cfgr| w.set_mco1sel(Mcosel::PLL1_Q)); + // configure pin for clock out + let mut mco = embassy_stm32::gpio::Flex::new(p.PA9); + mco.set_as_af_unchecked(0, AfType::output(OutputType::PushPull, Speed::High)); + + // This example assumes we're using the NUCLE0-G0B1RE together with X-NUCLEO-DRP1M1 USB-C expansion board. + // We need to turn on USB C power delivery by interfacing with the TCPP03-M20 IC. + // This is done setting enable high and by sending a command over I2C. + // See the TCPP03-M20 datasheet for more details. + + let mut enable = Output::new(p.PC8, Level::High, Speed::Low); + enable.set_high(); + + Timer::after_millis(1000).await; + + // i2c + // SCL: PB8 + // SDA: PB9 + let mut i2c = I2c::new( + p.I2C1, + p.PB8, + p.PB9, + Irqs, + p.DMA1_CH1, + p.DMA1_CH2, + Hertz(100_000), + Default::default(), + ); + + let i2c_address: u8 = 0x68 >> 1; //0b00110_100; // 7 bits address 0110 10x + + // We have to turn on the GDP switches to enable power from SOURCE + let reg = 0; + let value: u8 = 0b00011100; + + i2c.write(i2c_address, &[reg, value]).await.unwrap(); + + let reg = 0x1; + let mut read_buf = [255u8; 1]; + i2c.write_read(i2c_address, &[reg], &mut read_buf).await.unwrap(); + + debug!("TCPP03-M20 Value 1: {:02X}", read_buf[0]); + + // Create the driver, from the HAL. + let mut usbhost = UsbHost::new(*p.USB, Irqs, *p.PA12, *p.PA11); + + // info!("Start USB driver"); + usbhost.start(); + // let mut host = UsbHost::new(driver); + + debug!("Detecting device"); + // Wait for root-port to detect device + let speed = loop { + match usbhost.wait_for_device_event().await { + Connected(speed) => break speed, + _ => {} + } + }; + + println!("Found device with speed = {:?}", speed); + + let enum_info = usbhost.enumerate_root_bare(speed, 1).await.unwrap(); + let mut kbd = KbdHandler::try_register(&usbhost, &enum_info) + .await + .expect("Couldn't register keyboard"); + + loop { + let result = kbd.wait_for_event().await; + debug!("{}", result); + } +}