Extract synopsys otg driver

1 files changed, 1300 insertions, 0 deletions

diff --git a/embassy-usb-synopsys-otg/src/lib.rs b/embassy-usb-synopsys-otg/src/lib.rs
new file mode 100644
index 000000000..6d6cc9b0f
--- /dev/null
+++ b/embassy-usb-synopsys-otg/src/lib.rs
@@ -0,0 +1,1300 @@
+#![cfg_attr(not(test), no_std)]
+#![allow(async_fn_in_trait)]
+#![doc = include_str!("../README.md")]
+#![warn(missing_docs)]
+
+// This must go FIRST so that all the other modules see its macros.
+mod fmt;
+
+use core::cell::UnsafeCell;
+use core::marker::PhantomData;
+use core::sync::atomic::{AtomicBool, AtomicU16, Ordering};
+use core::task::Poll;
+
+use embassy_sync::waitqueue::AtomicWaker;
+use embassy_usb_driver::{
+    Bus as _, Direction, EndpointAddress, EndpointAllocError, EndpointError, EndpointIn, EndpointInfo, EndpointOut,
+    EndpointType, Event, Unsupported,
+};
+use futures::future::poll_fn;
+
+pub mod otg_v1;
+
+use otg_v1::{regs, vals, Otg};
+
+/// Handle interrupts.
+pub unsafe fn on_interrupt(r: Otg, state: &State<{ MAX_EP_COUNT }>, ep_count: usize, quirk_setup_late_cnak: bool) {
+    let ints = r.gintsts().read();
+    if ints.wkupint() || ints.usbsusp() || ints.usbrst() || ints.enumdne() || ints.otgint() || ints.srqint() {
+        // Mask interrupts and notify `Bus` to process them
+        r.gintmsk().write(|_| {});
+        state.bus_waker.wake();
+    }
+
+    // Handle RX
+    while r.gintsts().read().rxflvl() {
+        let status = r.grxstsp().read();
+        trace!("=== status {:08x}", status.0);
+        let ep_num = status.epnum() as usize;
+        let len = status.bcnt() as usize;
+
+        assert!(ep_num < ep_count);
+
+        match status.pktstsd() {
+            vals::Pktstsd::SETUP_DATA_RX => {
+                trace!("SETUP_DATA_RX");
+                assert!(len == 8, "invalid SETUP packet length={}", len);
+                assert!(ep_num == 0, "invalid SETUP packet endpoint={}", ep_num);
+
+                // flushing TX if something stuck in control endpoint
+                if r.dieptsiz(ep_num).read().pktcnt() != 0 {
+                    r.grstctl().modify(|w| {
+                        w.set_txfnum(ep_num as _);
+                        w.set_txfflsh(true);
+                    });
+                    while r.grstctl().read().txfflsh() {}
+                }
+
+                if state.ep0_setup_ready.load(Ordering::Relaxed) == false {
+                    // SAFETY: exclusive access ensured by atomic bool
+                    let data = unsafe { &mut *state.ep0_setup_data.get() };
+                    data[0..4].copy_from_slice(&r.fifo(0).read().0.to_ne_bytes());
+                    data[4..8].copy_from_slice(&r.fifo(0).read().0.to_ne_bytes());
+                    state.ep0_setup_ready.store(true, Ordering::Release);
+                    state.ep_out_wakers[0].wake();
+                } else {
+                    error!("received SETUP before previous finished processing");
+                    // discard FIFO
+                    r.fifo(0).read();
+                    r.fifo(0).read();
+                }
+            }
+            vals::Pktstsd::OUT_DATA_RX => {
+                trace!("OUT_DATA_RX ep={} len={}", ep_num, len);
+
+                if state.ep_out_size[ep_num].load(Ordering::Acquire) == EP_OUT_BUFFER_EMPTY {
+                    // SAFETY: Buffer size is allocated to be equal to endpoint's maximum packet size
+                    // We trust the peripheral to not exceed its configured MPSIZ
+                    let buf = unsafe { core::slice::from_raw_parts_mut(*state.ep_out_buffers[ep_num].get(), len) };
+
+                    for chunk in buf.chunks_mut(4) {
+                        // RX FIFO is shared so always read from fifo(0)
+                        let data = r.fifo(0).read().0;
+                        chunk.copy_from_slice(&data.to_ne_bytes()[0..chunk.len()]);
+                    }
+
+                    state.ep_out_size[ep_num].store(len as u16, Ordering::Release);
+                    state.ep_out_wakers[ep_num].wake();
+                } else {
+                    error!("ep_out buffer overflow index={}", ep_num);
+
+                    // discard FIFO data
+                    let len_words = (len + 3) / 4;
+                    for _ in 0..len_words {
+                        r.fifo(0).read().data();
+                    }
+                }
+            }
+            vals::Pktstsd::OUT_DATA_DONE => {
+                trace!("OUT_DATA_DONE ep={}", ep_num);
+            }
+            vals::Pktstsd::SETUP_DATA_DONE => {
+                trace!("SETUP_DATA_DONE ep={}", ep_num);
+
+                if quirk_setup_late_cnak {
+                    // Clear NAK to indicate we are ready to receive more data
+                    r.doepctl(ep_num).modify(|w| w.set_cnak(true));
+                }
+            }
+            x => trace!("unknown PKTSTS: {}", x.to_bits()),
+        }
+    }
+
+    // IN endpoint interrupt
+    if ints.iepint() {
+        let mut ep_mask = r.daint().read().iepint();
+        let mut ep_num = 0;
+
+        // Iterate over endpoints while there are non-zero bits in the mask
+        while ep_mask != 0 {
+            if ep_mask & 1 != 0 {
+                let ep_ints = r.diepint(ep_num).read();
+
+                // clear all
+                r.diepint(ep_num).write_value(ep_ints);
+
+                // TXFE is cleared in DIEPEMPMSK
+                if ep_ints.txfe() {
+                    critical_section::with(|_| {
+                        r.diepempmsk().modify(|w| {
+                            w.set_ineptxfem(w.ineptxfem() & !(1 << ep_num));
+                        });
+                    });
+                }
+
+                state.ep_in_wakers[ep_num].wake();
+                trace!("in ep={} irq val={:08x}", ep_num, ep_ints.0);
+            }
+
+            ep_mask >>= 1;
+            ep_num += 1;
+        }
+    }
+
+    // not needed? reception handled in rxflvl
+    // OUT endpoint interrupt
+    // if ints.oepint() {
+    //     let mut ep_mask = r.daint().read().oepint();
+    //     let mut ep_num = 0;
+
+    //     while ep_mask != 0 {
+    //         if ep_mask & 1 != 0 {
+    //             let ep_ints = r.doepint(ep_num).read();
+    //             // clear all
+    //             r.doepint(ep_num).write_value(ep_ints);
+    //             state.ep_out_wakers[ep_num].wake();
+    //             trace!("out ep={} irq val={:08x}", ep_num, ep_ints.0);
+    //         }
+
+    //         ep_mask >>= 1;
+    //         ep_num += 1;
+    //     }
+    // }
+}
+
+/// USB PHY type
+#[derive(Copy, Clone, Debug, Eq, PartialEq)]
+pub enum PhyType {
+    /// Internal Full-Speed PHY
+    ///
+    /// Available on most High-Speed peripherals.
+    InternalFullSpeed,
+    /// Internal High-Speed PHY
+    ///
+    /// Available on a few STM32 chips.
+    InternalHighSpeed,
+    /// External ULPI High-Speed PHY
+    ExternalHighSpeed,
+}
+
+impl PhyType {
+    /// Get whether this PHY is any of the internal types.
+    pub fn internal(&self) -> bool {
+        match self {
+            PhyType::InternalFullSpeed | PhyType::InternalHighSpeed => true,
+            PhyType::ExternalHighSpeed => false,
+        }
+    }
+
+    /// Get whether this PHY is any of the high-speed types.
+    pub fn high_speed(&self) -> bool {
+        match self {
+            PhyType::InternalFullSpeed => false,
+            PhyType::ExternalHighSpeed | PhyType::InternalHighSpeed => true,
+        }
+    }
+
+    fn to_dspd(&self) -> vals::Dspd {
+        match self {
+            PhyType::InternalFullSpeed => vals::Dspd::FULL_SPEED_INTERNAL,
+            PhyType::InternalHighSpeed => vals::Dspd::HIGH_SPEED,
+            PhyType::ExternalHighSpeed => vals::Dspd::HIGH_SPEED,
+        }
+    }
+}
+
+/// Indicates that [State::ep_out_buffers] is empty.
+const EP_OUT_BUFFER_EMPTY: u16 = u16::MAX;
+
+/// USB OTG driver state.
+pub struct State<const EP_COUNT: usize> {
+    /// Holds received SETUP packets. Available if [State::ep0_setup_ready] is true.
+    ep0_setup_data: UnsafeCell<[u8; 8]>,
+    ep0_setup_ready: AtomicBool,
+    ep_in_wakers: [AtomicWaker; EP_COUNT],
+    ep_out_wakers: [AtomicWaker; EP_COUNT],
+    /// RX FIFO is shared so extra buffers are needed to dequeue all data without waiting on each endpoint.
+    /// Buffers are ready when associated [State::ep_out_size] != [EP_OUT_BUFFER_EMPTY].
+    ep_out_buffers: [UnsafeCell<*mut u8>; EP_COUNT],
+    ep_out_size: [AtomicU16; EP_COUNT],
+    bus_waker: AtomicWaker,
+}
+
+unsafe impl<const EP_COUNT: usize> Send for State<EP_COUNT> {}
+unsafe impl<const EP_COUNT: usize> Sync for State<EP_COUNT> {}
+
+impl<const EP_COUNT: usize> State<EP_COUNT> {
+    /// Create a new State.
+    pub const fn new() -> Self {
+        const NEW_AW: AtomicWaker = AtomicWaker::new();
+        const NEW_BUF: UnsafeCell<*mut u8> = UnsafeCell::new(0 as _);
+        const NEW_SIZE: AtomicU16 = AtomicU16::new(EP_OUT_BUFFER_EMPTY);
+
+        Self {
+            ep0_setup_data: UnsafeCell::new([0u8; 8]),
+            ep0_setup_ready: AtomicBool::new(false),
+            ep_in_wakers: [NEW_AW; EP_COUNT],
+            ep_out_wakers: [NEW_AW; EP_COUNT],
+            ep_out_buffers: [NEW_BUF; EP_COUNT],
+            ep_out_size: [NEW_SIZE; EP_COUNT],
+            bus_waker: NEW_AW,
+        }
+    }
+}
+
+#[derive(Debug, Clone, Copy)]
+struct EndpointData {
+    ep_type: EndpointType,
+    max_packet_size: u16,
+    fifo_size_words: u16,
+}
+
+/// USB driver config.
+#[non_exhaustive]
+#[derive(Clone, Copy, PartialEq, Eq, Debug)]
+pub struct Config {
+    /// Enable VBUS detection.
+    ///
+    /// The USB spec requires USB devices monitor for USB cable plug/unplug and react accordingly.
+    /// This is done by checking whether there is 5V on the VBUS pin or not.
+    ///
+    /// If your device is bus-powered (powers itself from the USB host via VBUS), then this is optional.
+    /// (If there's no power in VBUS your device would be off anyway, so it's fine to always assume
+    /// there's power in VBUS, i.e. the USB cable is always plugged in.)
+    ///
+    /// If your device is self-powered (i.e. it gets power from a source other than the USB cable, and
+    /// therefore can stay powered through USB cable plug/unplug) then you MUST set this to true.
+    ///
+    /// If you set this to true, you must connect VBUS to PA9 for FS, PB13 for HS, possibly with a
+    /// voltage divider. See ST application note AN4879 and the reference manual for more details.
+    pub vbus_detection: bool,
+}
+
+impl Default for Config {
+    fn default() -> Self {
+        Self { vbus_detection: true }
+    }
+}
+
+/// USB driver.
+pub struct Driver<'d> {
+    config: Config,
+    ep_in: [Option<EndpointData>; MAX_EP_COUNT],
+    ep_out: [Option<EndpointData>; MAX_EP_COUNT],
+    ep_out_buffer: &'d mut [u8],
+    ep_out_buffer_offset: usize,
+    instance: OtgInstance<'d>,
+}
+
+impl<'d> Driver<'d> {
+    /// Initializes USB OTG peripheral.
+    ///
+    /// # Arguments
+    ///
+    /// * `ep_out_buffer` - An internal buffer used to temporarily store received packets.
+    /// Must be large enough to fit all OUT endpoint max packet sizes.
+    /// Endpoint allocation will fail if it is too small.
+    pub fn new(ep_out_buffer: &'d mut [u8], instance: OtgInstance<'d>, config: Config) -> Self {
+        Self {
+            config,
+            ep_in: [None; MAX_EP_COUNT],
+            ep_out: [None; MAX_EP_COUNT],
+            ep_out_buffer,
+            ep_out_buffer_offset: 0,
+            instance,
+        }
+    }
+
+    // Returns total amount of words (u32) allocated in dedicated FIFO
+    fn allocated_fifo_words(&self) -> u16 {
+        self.instance.extra_rx_fifo_words + ep_fifo_size(&self.ep_out) + ep_fifo_size(&self.ep_in)
+    }
+
+    /// Creates an [`Endpoint`] with the given parameters.
+    pub fn alloc_endpoint<D: Dir>(
+        &mut self,
+        ep_type: EndpointType,
+        max_packet_size: u16,
+        interval_ms: u8,
+    ) -> Result<Endpoint<'d, D>, EndpointAllocError> {
+        trace!(
+            "allocating type={:?} mps={:?} interval_ms={}, dir={:?}",
+            ep_type,
+            max_packet_size,
+            interval_ms,
+            D::dir()
+        );
+
+        if D::dir() == Direction::Out {
+            if self.ep_out_buffer_offset + max_packet_size as usize >= self.ep_out_buffer.len() {
+                error!("Not enough endpoint out buffer capacity");
+                return Err(EndpointAllocError);
+            }
+        };
+
+        let fifo_size_words = match D::dir() {
+            Direction::Out => (max_packet_size + 3) / 4,
+            // INEPTXFD requires minimum size of 16 words
+            Direction::In => u16::max((max_packet_size + 3) / 4, 16),
+        };
+
+        if fifo_size_words + self.allocated_fifo_words() > self.instance.fifo_depth_words {
+            error!("Not enough FIFO capacity");
+            return Err(EndpointAllocError);
+        }
+
+        let eps = match D::dir() {
+            Direction::Out => &mut self.ep_out,
+            Direction::In => &mut self.ep_in,
+        };
+
+        // Find free endpoint slot
+        let slot = eps.iter_mut().enumerate().find(|(i, ep)| {
+            if *i == 0 && ep_type != EndpointType::Control {
+                // reserved for control pipe
+                false
+            } else {
+                ep.is_none()
+            }
+        });
+
+        let index = match slot {
+            Some((index, ep)) => {
+                *ep = Some(EndpointData {
+                    ep_type,
+                    max_packet_size,
+                    fifo_size_words,
+                });
+                index
+            }
+            None => {
+                error!("No free endpoints available");
+                return Err(EndpointAllocError);
+            }
+        };
+
+        trace!("  index={}", index);
+
+        if D::dir() == Direction::Out {
+            // Buffer capacity check was done above, now allocation cannot fail
+            unsafe {
+                *self.instance.state.ep_out_buffers[index].get() =
+                    self.ep_out_buffer.as_mut_ptr().offset(self.ep_out_buffer_offset as _);
+            }
+            self.ep_out_buffer_offset += max_packet_size as usize;
+        }
+
+        Ok(Endpoint {
+            _phantom: PhantomData,
+            regs: self.instance.regs,
+            state: self.instance.state,
+            info: EndpointInfo {
+                addr: EndpointAddress::from_parts(index, D::dir()),
+                ep_type,
+                max_packet_size,
+                interval_ms,
+            },
+        })
+    }
+}
+
+impl<'d> embassy_usb_driver::Driver<'d> for Driver<'d> {
+    type EndpointOut = Endpoint<'d, Out>;
+    type EndpointIn = Endpoint<'d, In>;
+    type ControlPipe = ControlPipe<'d>;
+    type Bus = Bus<'d>;
+
+    fn alloc_endpoint_in(
+        &mut self,
+        ep_type: EndpointType,
+        max_packet_size: u16,
+        interval_ms: u8,
+    ) -> Result<Self::EndpointIn, EndpointAllocError> {
+        self.alloc_endpoint(ep_type, max_packet_size, interval_ms)
+    }
+
+    fn alloc_endpoint_out(
+        &mut self,
+        ep_type: EndpointType,
+        max_packet_size: u16,
+        interval_ms: u8,
+    ) -> Result<Self::EndpointOut, EndpointAllocError> {
+        self.alloc_endpoint(ep_type, max_packet_size, interval_ms)
+    }
+
+    fn start(mut self, control_max_packet_size: u16) -> (Self::Bus, Self::ControlPipe) {
+        let ep_out = self
+            .alloc_endpoint(EndpointType::Control, control_max_packet_size, 0)
+            .unwrap();
+        let ep_in = self
+            .alloc_endpoint(EndpointType::Control, control_max_packet_size, 0)
+            .unwrap();
+        assert_eq!(ep_out.info.addr.index(), 0);
+        assert_eq!(ep_in.info.addr.index(), 0);
+
+        trace!("start");
+
+        let regs = self.instance.regs;
+        let quirk_setup_late_cnak = self.instance.quirk_setup_late_cnak;
+        (
+            Bus {
+                config: self.config,
+                ep_in: self.ep_in,
+                ep_out: self.ep_out,
+                inited: false,
+                instance: self.instance,
+            },
+            ControlPipe {
+                max_packet_size: control_max_packet_size,
+                ep_out,
+                ep_in,
+                regs,
+                quirk_setup_late_cnak,
+            },
+        )
+    }
+}
+
+/// USB bus.
+pub struct Bus<'d> {
+    config: Config,
+    ep_in: [Option<EndpointData>; MAX_EP_COUNT],
+    ep_out: [Option<EndpointData>; MAX_EP_COUNT],
+    instance: OtgInstance<'d>,
+    inited: bool,
+}
+
+impl<'d> Bus<'d> {
+    fn restore_irqs(&mut self) {
+        self.instance.regs.gintmsk().write(|w| {
+            w.set_usbrst(true);
+            w.set_enumdnem(true);
+            w.set_usbsuspm(true);
+            w.set_wuim(true);
+            w.set_iepint(true);
+            w.set_oepint(true);
+            w.set_rxflvlm(true);
+            w.set_srqim(true);
+            w.set_otgint(true);
+        });
+    }
+}
+
+impl<'d> Bus<'d> {
+    /// Returns the PHY type.
+    pub fn phy_type(&self) -> PhyType {
+        self.instance.phy_type
+    }
+
+    /// Configures the PHY as a device.
+    pub fn configure_as_device(&mut self) {
+        let r = self.instance.regs;
+        let phy_type = self.instance.phy_type;
+        r.gusbcfg().write(|w| {
+            // Force device mode
+            w.set_fdmod(true);
+            // Enable internal full-speed PHY
+            w.set_physel(phy_type.internal() && !phy_type.high_speed());
+        });
+    }
+
+    /// Applies configuration specific to
+    /// Core ID 0x0000_1100 and 0x0000_1200
+    pub fn config_v1(&mut self) {
+        let r = self.instance.regs;
+        let phy_type = self.instance.phy_type;
+        assert!(phy_type != PhyType::InternalHighSpeed);
+
+        r.gccfg_v1().modify(|w| {
+            // Enable internal full-speed PHY, logic is inverted
+            w.set_pwrdwn(phy_type.internal());
+        });
+
+        // F429-like chips have the GCCFG.NOVBUSSENS bit
+        r.gccfg_v1().modify(|w| {
+            w.set_novbussens(!self.config.vbus_detection);
+            w.set_vbusasen(false);
+            w.set_vbusbsen(self.config.vbus_detection);
+            w.set_sofouten(false);
+        });
+    }
+
+    /// Applies configuration specific to
+    /// Core ID 0x0000_2000, 0x0000_2100, 0x0000_2300, 0x0000_3000 and 0x0000_3100
+    pub fn config_v2v3(&mut self) {
+        let r = self.instance.regs;
+        let phy_type = self.instance.phy_type;
+
+        // F446-like chips have the GCCFG.VBDEN bit with the opposite meaning
+        r.gccfg_v2().modify(|w| {
+            // Enable internal full-speed PHY, logic is inverted
+            w.set_pwrdwn(phy_type.internal() && !phy_type.high_speed());
+            w.set_phyhsen(phy_type.internal() && phy_type.high_speed());
+        });
+
+        r.gccfg_v2().modify(|w| {
+            w.set_vbden(self.config.vbus_detection);
+        });
+
+        // Force B-peripheral session
+        r.gotgctl().modify(|w| {
+            w.set_bvaloen(!self.config.vbus_detection);
+            w.set_bvaloval(true);
+        });
+    }
+
+    fn init(&mut self) {
+        let r = self.instance.regs;
+        let phy_type = self.instance.phy_type;
+
+        // Soft disconnect.
+        r.dctl().write(|w| w.set_sdis(true));
+
+        // Set speed.
+        r.dcfg().write(|w| {
+            w.set_pfivl(vals::Pfivl::FRAME_INTERVAL_80);
+            w.set_dspd(phy_type.to_dspd());
+        });
+
+        // Unmask transfer complete EP interrupt
+        r.diepmsk().write(|w| {
+            w.set_xfrcm(true);
+        });
+
+        // Unmask and clear core interrupts
+        self.restore_irqs();
+        r.gintsts().write_value(regs::Gintsts(0xFFFF_FFFF));
+
+        // Unmask global interrupt
+        r.gahbcfg().write(|w| {
+            w.set_gint(true); // unmask global interrupt
+        });
+
+        // Connect
+        r.dctl().write(|w| w.set_sdis(false));
+    }
+
+    fn init_fifo(&mut self) {
+        trace!("init_fifo");
+
+        let regs = self.instance.regs;
+        // ERRATA NOTE: Don't interrupt FIFOs being written to. The interrupt
+        // handler COULD interrupt us here and do FIFO operations, so ensure
+        // the interrupt does not occur.
+        critical_section::with(|_| {
+            // Configure RX fifo size. All endpoints share the same FIFO area.
+            let rx_fifo_size_words = self.instance.extra_rx_fifo_words + ep_fifo_size(&self.ep_out);
+            trace!("configuring rx fifo size={}", rx_fifo_size_words);
+
+            regs.grxfsiz().modify(|w| w.set_rxfd(rx_fifo_size_words));
+
+            // Configure TX (USB in direction) fifo size for each endpoint
+            let mut fifo_top = rx_fifo_size_words;
+            for i in 0..self.instance.endpoint_count {
+                if let Some(ep) = self.ep_in[i] {
+                    trace!(
+                        "configuring tx fifo ep={}, offset={}, size={}",
+                        i,
+                        fifo_top,
+                        ep.fifo_size_words
+                    );
+
+                    let dieptxf = if i == 0 { regs.dieptxf0() } else { regs.dieptxf(i - 1) };
+
+                    dieptxf.write(|w| {
+                        w.set_fd(ep.fifo_size_words);
+                        w.set_sa(fifo_top);
+                    });
+
+                    fifo_top += ep.fifo_size_words;
+                }
+            }
+
+            assert!(
+                fifo_top <= self.instance.fifo_depth_words,
+                "FIFO allocations exceeded maximum capacity"
+            );
+
+            // Flush fifos
+            regs.grstctl().write(|w| {
+                w.set_rxfflsh(true);
+                w.set_txfflsh(true);
+                w.set_txfnum(0x10);
+            });
+        });
+
+        loop {
+            let x = regs.grstctl().read();
+            if !x.rxfflsh() && !x.txfflsh() {
+                break;
+            }
+        }
+    }
+
+    fn configure_endpoints(&mut self) {
+        trace!("configure_endpoints");
+
+        let regs = self.instance.regs;
+
+        // Configure IN endpoints
+        for (index, ep) in self.ep_in.iter().enumerate() {
+            if let Some(ep) = ep {
+                critical_section::with(|_| {
+                    regs.diepctl(index).write(|w| {
+                        if index == 0 {
+                            w.set_mpsiz(ep0_mpsiz(ep.max_packet_size));
+                        } else {
+                            w.set_mpsiz(ep.max_packet_size);
+                            w.set_eptyp(to_eptyp(ep.ep_type));
+                            w.set_sd0pid_sevnfrm(true);
+                            w.set_txfnum(index as _);
+                            w.set_snak(true);
+                        }
+                    });
+                });
+            }
+        }
+
+        // Configure OUT endpoints
+        for (index, ep) in self.ep_out.iter().enumerate() {
+            if let Some(ep) = ep {
+                critical_section::with(|_| {
+                    regs.doepctl(index).write(|w| {
+                        if index == 0 {
+                            w.set_mpsiz(ep0_mpsiz(ep.max_packet_size));
+                        } else {
+                            w.set_mpsiz(ep.max_packet_size);
+                            w.set_eptyp(to_eptyp(ep.ep_type));
+                            w.set_sd0pid_sevnfrm(true);
+                        }
+                    });
+
+                    regs.doeptsiz(index).modify(|w| {
+                        w.set_xfrsiz(ep.max_packet_size as _);
+                        if index == 0 {
+                            w.set_rxdpid_stupcnt(1);
+                        } else {
+                            w.set_pktcnt(1);
+                        }
+                    });
+                });
+            }
+        }
+
+        // Enable IRQs for allocated endpoints
+        regs.daintmsk().modify(|w| {
+            w.set_iepm(ep_irq_mask(&self.ep_in));
+            // OUT interrupts not used, handled in RXFLVL
+            // w.set_oepm(ep_irq_mask(&self.ep_out));
+        });
+    }
+
+    fn disable_all_endpoints(&mut self) {
+        for i in 0..self.instance.endpoint_count {
+            self.endpoint_set_enabled(EndpointAddress::from_parts(i, Direction::In), false);
+            self.endpoint_set_enabled(EndpointAddress::from_parts(i, Direction::Out), false);
+        }
+    }
+}
+
+impl<'d> embassy_usb_driver::Bus for Bus<'d> {
+    async fn poll(&mut self) -> Event {
+        poll_fn(move |cx| {
+            if !self.inited {
+                self.init();
+                self.inited = true;
+
+                // If no vbus detection, just return a single PowerDetected event at startup.
+                if !self.config.vbus_detection {
+                    return Poll::Ready(Event::PowerDetected);
+                }
+            }
+
+            let regs = self.instance.regs;
+            self.instance.state.bus_waker.register(cx.waker());
+
+            let ints = regs.gintsts().read();
+
+            if ints.srqint() {
+                trace!("vbus detected");
+
+                regs.gintsts().write(|w| w.set_srqint(true)); // clear
+                self.restore_irqs();
+
+                if self.config.vbus_detection {
+                    return Poll::Ready(Event::PowerDetected);
+                }
+            }
+
+            if ints.otgint() {
+                let otgints = regs.gotgint().read();
+                regs.gotgint().write_value(otgints); // clear all
+                self.restore_irqs();
+
+                if otgints.sedet() {
+                    trace!("vbus removed");
+                    if self.config.vbus_detection {
+                        self.disable_all_endpoints();
+                        return Poll::Ready(Event::PowerRemoved);
+                    }
+                }
+            }
+
+            if ints.usbrst() {
+                trace!("reset");
+
+                self.init_fifo();
+                self.configure_endpoints();
+
+                // Reset address
+                critical_section::with(|_| {
+                    regs.dcfg().modify(|w| {
+                        w.set_dad(0);
+                    });
+                });
+
+                regs.gintsts().write(|w| w.set_usbrst(true)); // clear
+                self.restore_irqs();
+            }
+
+            if ints.enumdne() {
+                trace!("enumdne");
+
+                let speed = regs.dsts().read().enumspd();
+                let trdt = (self.instance.calculate_trdt_fn)(speed);
+                trace!("  speed={} trdt={}", speed.to_bits(), trdt);
+                regs.gusbcfg().modify(|w| w.set_trdt(trdt));
+
+                regs.gintsts().write(|w| w.set_enumdne(true)); // clear
+                self.restore_irqs();
+
+                return Poll::Ready(Event::Reset);
+            }
+
+            if ints.usbsusp() {
+                trace!("suspend");
+                regs.gintsts().write(|w| w.set_usbsusp(true)); // clear
+                self.restore_irqs();
+                return Poll::Ready(Event::Suspend);
+            }
+
+            if ints.wkupint() {
+                trace!("resume");
+                regs.gintsts().write(|w| w.set_wkupint(true)); // clear
+                self.restore_irqs();
+                return Poll::Ready(Event::Resume);
+            }
+
+            Poll::Pending
+        })
+        .await
+    }
+
+    fn endpoint_set_stalled(&mut self, ep_addr: EndpointAddress, stalled: bool) {
+        trace!("endpoint_set_stalled ep={:?} en={}", ep_addr, stalled);
+
+        assert!(
+            ep_addr.index() < self.instance.endpoint_count,
+            "endpoint_set_stalled index {} out of range",
+            ep_addr.index()
+        );
+
+        let regs = self.instance.regs;
+        let state = self.instance.state;
+        match ep_addr.direction() {
+            Direction::Out => {
+                critical_section::with(|_| {
+                    regs.doepctl(ep_addr.index()).modify(|w| {
+                        w.set_stall(stalled);
+                    });
+                });
+
+                state.ep_out_wakers[ep_addr.index()].wake();
+            }
+            Direction::In => {
+                critical_section::with(|_| {
+                    regs.diepctl(ep_addr.index()).modify(|w| {
+                        w.set_stall(stalled);
+                    });
+                });
+
+                state.ep_in_wakers[ep_addr.index()].wake();
+            }
+        }
+    }
+
+    fn endpoint_is_stalled(&mut self, ep_addr: EndpointAddress) -> bool {
+        assert!(
+            ep_addr.index() < self.instance.endpoint_count,
+            "endpoint_is_stalled index {} out of range",
+            ep_addr.index()
+        );
+
+        let regs = self.instance.regs;
+        match ep_addr.direction() {
+            Direction::Out => regs.doepctl(ep_addr.index()).read().stall(),
+            Direction::In => regs.diepctl(ep_addr.index()).read().stall(),
+        }
+    }
+
+    fn endpoint_set_enabled(&mut self, ep_addr: EndpointAddress, enabled: bool) {
+        trace!("endpoint_set_enabled ep={:?} en={}", ep_addr, enabled);
+
+        assert!(
+            ep_addr.index() < self.instance.endpoint_count,
+            "endpoint_set_enabled index {} out of range",
+            ep_addr.index()
+        );
+
+        let regs = self.instance.regs;
+        let state = self.instance.state;
+        match ep_addr.direction() {
+            Direction::Out => {
+                critical_section::with(|_| {
+                    // cancel transfer if active
+                    if !enabled && regs.doepctl(ep_addr.index()).read().epena() {
+                        regs.doepctl(ep_addr.index()).modify(|w| {
+                            w.set_snak(true);
+                            w.set_epdis(true);
+                        })
+                    }
+
+                    regs.doepctl(ep_addr.index()).modify(|w| {
+                        w.set_usbaep(enabled);
+                    });
+
+                    // Flush tx fifo
+                    regs.grstctl().write(|w| {
+                        w.set_txfflsh(true);
+                        w.set_txfnum(ep_addr.index() as _);
+                    });
+                    loop {
+                        let x = regs.grstctl().read();
+                        if !x.txfflsh() {
+                            break;
+                        }
+                    }
+                });
+
+                // Wake `Endpoint::wait_enabled()`
+                state.ep_out_wakers[ep_addr.index()].wake();
+            }
+            Direction::In => {
+                critical_section::with(|_| {
+                    // cancel transfer if active
+                    if !enabled && regs.diepctl(ep_addr.index()).read().epena() {
+                        regs.diepctl(ep_addr.index()).modify(|w| {
+                            w.set_snak(true); // set NAK
+                            w.set_epdis(true);
+                        })
+                    }
+
+                    regs.diepctl(ep_addr.index()).modify(|w| {
+                        w.set_usbaep(enabled);
+                        w.set_cnak(enabled); // clear NAK that might've been set by SNAK above.
+                    })
+                });
+
+                // Wake `Endpoint::wait_enabled()`
+                state.ep_in_wakers[ep_addr.index()].wake();
+            }
+        }
+    }
+
+    async fn enable(&mut self) {
+        trace!("enable");
+        // TODO: enable the peripheral once enable/disable semantics are cleared up in embassy-usb
+    }
+
+    async fn disable(&mut self) {
+        trace!("disable");
+
+        // TODO: disable the peripheral once enable/disable semantics are cleared up in embassy-usb
+        //Bus::disable(self);
+    }
+
+    async fn remote_wakeup(&mut self) -> Result<(), Unsupported> {
+        Err(Unsupported)
+    }
+}
+
+/// USB endpoint direction.
+pub trait Dir {
+    /// Returns the direction value.
+    fn dir() -> Direction;
+}
+
+/// Marker type for the "IN" direction.
+pub enum In {}
+impl Dir for In {
+    fn dir() -> Direction {
+        Direction::In
+    }
+}
+
+/// Marker type for the "OUT" direction.
+pub enum Out {}
+impl Dir for Out {
+    fn dir() -> Direction {
+        Direction::Out
+    }
+}
+
+/// USB endpoint.
+pub struct Endpoint<'d, D> {
+    _phantom: PhantomData<D>,
+    regs: Otg,
+    info: EndpointInfo,
+    state: &'d State<{ MAX_EP_COUNT }>,
+}
+
+impl<'d> embassy_usb_driver::Endpoint for Endpoint<'d, In> {
+    fn info(&self) -> &EndpointInfo {
+        &self.info
+    }
+
+    async fn wait_enabled(&mut self) {
+        poll_fn(|cx| {
+            let ep_index = self.info.addr.index();
+
+            self.state.ep_in_wakers[ep_index].register(cx.waker());
+
+            if self.regs.diepctl(ep_index).read().usbaep() {
+                Poll::Ready(())
+            } else {
+                Poll::Pending
+            }
+        })
+        .await
+    }
+}
+
+impl<'d> embassy_usb_driver::Endpoint for Endpoint<'d, Out> {
+    fn info(&self) -> &EndpointInfo {
+        &self.info
+    }
+
+    async fn wait_enabled(&mut self) {
+        poll_fn(|cx| {
+            let ep_index = self.info.addr.index();
+
+            self.state.ep_out_wakers[ep_index].register(cx.waker());
+
+            if self.regs.doepctl(ep_index).read().usbaep() {
+                Poll::Ready(())
+            } else {
+                Poll::Pending
+            }
+        })
+        .await
+    }
+}
+
+impl<'d> embassy_usb_driver::EndpointOut for Endpoint<'d, Out> {
+    async fn read(&mut self, buf: &mut [u8]) -> Result<usize, EndpointError> {
+        trace!("read start len={}", buf.len());
+
+        poll_fn(|cx| {
+            let index = self.info.addr.index();
+            self.state.ep_out_wakers[index].register(cx.waker());
+
+            let doepctl = self.regs.doepctl(index).read();
+            trace!("read ep={:?}: doepctl {:08x}", self.info.addr, doepctl.0,);
+            if !doepctl.usbaep() {
+                trace!("read ep={:?} error disabled", self.info.addr);
+                return Poll::Ready(Err(EndpointError::Disabled));
+            }
+
+            let len = self.state.ep_out_size[index].load(Ordering::Relaxed);
+            if len != EP_OUT_BUFFER_EMPTY {
+                trace!("read ep={:?} done len={}", self.info.addr, len);
+
+                if len as usize > buf.len() {
+                    return Poll::Ready(Err(EndpointError::BufferOverflow));
+                }
+
+                // SAFETY: exclusive access ensured by `ep_out_size` atomic variable
+                let data =
+                    unsafe { core::slice::from_raw_parts(*self.state.ep_out_buffers[index].get(), len as usize) };
+                buf[..len as usize].copy_from_slice(data);
+
+                // Release buffer
+                self.state.ep_out_size[index].store(EP_OUT_BUFFER_EMPTY, Ordering::Release);
+
+                critical_section::with(|_| {
+                    // Receive 1 packet
+                    self.regs.doeptsiz(index).modify(|w| {
+                        w.set_xfrsiz(self.info.max_packet_size as _);
+                        w.set_pktcnt(1);
+                    });
+
+                    // Clear NAK to indicate we are ready to receive more data
+                    self.regs.doepctl(index).modify(|w| {
+                        w.set_cnak(true);
+                    });
+                });
+
+                Poll::Ready(Ok(len as usize))
+            } else {
+                Poll::Pending
+            }
+        })
+        .await
+    }
+}
+
+impl<'d> embassy_usb_driver::EndpointIn for Endpoint<'d, In> {
+    async fn write(&mut self, buf: &[u8]) -> Result<(), EndpointError> {
+        trace!("write ep={:?} data={:?}", self.info.addr, buf);
+
+        if buf.len() > self.info.max_packet_size as usize {
+            return Err(EndpointError::BufferOverflow);
+        }
+
+        let index = self.info.addr.index();
+        // Wait for previous transfer to complete and check if endpoint is disabled
+        poll_fn(|cx| {
+            self.state.ep_in_wakers[index].register(cx.waker());
+
+            let diepctl = self.regs.diepctl(index).read();
+            let dtxfsts = self.regs.dtxfsts(index).read();
+            trace!(
+                "write ep={:?}: diepctl {:08x} ftxfsts {:08x}",
+                self.info.addr,
+                diepctl.0,
+                dtxfsts.0
+            );
+            if !diepctl.usbaep() {
+                trace!("write ep={:?} wait for prev: error disabled", self.info.addr);
+                Poll::Ready(Err(EndpointError::Disabled))
+            } else if !diepctl.epena() {
+                trace!("write ep={:?} wait for prev: ready", self.info.addr);
+                Poll::Ready(Ok(()))
+            } else {
+                trace!("write ep={:?} wait for prev: pending", self.info.addr);
+                Poll::Pending
+            }
+        })
+        .await?;
+
+        if buf.len() > 0 {
+            poll_fn(|cx| {
+                self.state.ep_in_wakers[index].register(cx.waker());
+
+                let size_words = (buf.len() + 3) / 4;
+
+                let fifo_space = self.regs.dtxfsts(index).read().ineptfsav() as usize;
+                if size_words > fifo_space {
+                    // Not enough space in fifo, enable tx fifo empty interrupt
+                    critical_section::with(|_| {
+                        self.regs.diepempmsk().modify(|w| {
+                            w.set_ineptxfem(w.ineptxfem() | (1 << index));
+                        });
+                    });
+
+                    trace!("tx fifo for ep={} full, waiting for txfe", index);
+
+                    Poll::Pending
+                } else {
+                    trace!("write ep={:?} wait for fifo: ready", self.info.addr);
+                    Poll::Ready(())
+                }
+            })
+            .await
+        }
+
+        // ERRATA: Transmit data FIFO is corrupted when a write sequence to the FIFO is interrupted with
+        // accesses to certain OTG_FS registers.
+        //
+        // Prevent the interrupt (which might poke FIFOs) from executing while copying data to FIFOs.
+        critical_section::with(|_| {
+            // Setup transfer size
+            self.regs.dieptsiz(index).write(|w| {
+                w.set_mcnt(1);
+                w.set_pktcnt(1);
+                w.set_xfrsiz(buf.len() as _);
+            });
+
+            // Enable endpoint
+            self.regs.diepctl(index).modify(|w| {
+                w.set_cnak(true);
+                w.set_epena(true);
+            });
+
+            // Write data to FIFO
+            for chunk in buf.chunks(4) {
+                let mut tmp = [0u8; 4];
+                tmp[0..chunk.len()].copy_from_slice(chunk);
+                self.regs.fifo(index).write_value(regs::Fifo(u32::from_ne_bytes(tmp)));
+            }
+        });
+
+        trace!("write done ep={:?}", self.info.addr);
+
+        Ok(())
+    }
+}
+
+/// USB control pipe.
+pub struct ControlPipe<'d> {
+    max_packet_size: u16,
+    regs: Otg,
+    ep_in: Endpoint<'d, In>,
+    ep_out: Endpoint<'d, Out>,
+    quirk_setup_late_cnak: bool,
+}
+
+impl<'d> embassy_usb_driver::ControlPipe for ControlPipe<'d> {
+    fn max_packet_size(&self) -> usize {
+        usize::from(self.max_packet_size)
+    }
+
+    async fn setup(&mut self) -> [u8; 8] {
+        poll_fn(|cx| {
+            self.ep_out.state.ep_out_wakers[0].register(cx.waker());
+
+            if self.ep_out.state.ep0_setup_ready.load(Ordering::Relaxed) {
+                let data = unsafe { *self.ep_out.state.ep0_setup_data.get() };
+                self.ep_out.state.ep0_setup_ready.store(false, Ordering::Release);
+
+                // EP0 should not be controlled by `Bus` so this RMW does not need a critical section
+                // Receive 1 SETUP packet
+                self.regs.doeptsiz(self.ep_out.info.addr.index()).modify(|w| {
+                    w.set_rxdpid_stupcnt(1);
+                });
+
+                // Clear NAK to indicate we are ready to receive more data
+                if !self.quirk_setup_late_cnak {
+                    self.regs
+                        .doepctl(self.ep_out.info.addr.index())
+                        .modify(|w| w.set_cnak(true));
+                }
+
+                trace!("SETUP received: {:?}", data);
+                Poll::Ready(data)
+            } else {
+                trace!("SETUP waiting");
+                Poll::Pending
+            }
+        })
+        .await
+    }
+
+    async fn data_out(&mut self, buf: &mut [u8], _first: bool, _last: bool) -> Result<usize, EndpointError> {
+        trace!("control: data_out");
+        let len = self.ep_out.read(buf).await?;
+        trace!("control: data_out read: {:?}", &buf[..len]);
+        Ok(len)
+    }
+
+    async fn data_in(&mut self, data: &[u8], _first: bool, last: bool) -> Result<(), EndpointError> {
+        trace!("control: data_in write: {:?}", data);
+        self.ep_in.write(data).await?;
+
+        // wait for status response from host after sending the last packet
+        if last {
+            trace!("control: data_in waiting for status");
+            self.ep_out.read(&mut []).await?;
+            trace!("control: complete");
+        }
+
+        Ok(())
+    }
+
+    async fn accept(&mut self) {
+        trace!("control: accept");
+
+        self.ep_in.write(&[]).await.ok();
+
+        trace!("control: accept OK");
+    }
+
+    async fn reject(&mut self) {
+        trace!("control: reject");
+
+        // EP0 should not be controlled by `Bus` so this RMW does not need a critical section
+        self.regs.diepctl(self.ep_in.info.addr.index()).modify(|w| {
+            w.set_stall(true);
+        });
+        self.regs.doepctl(self.ep_out.info.addr.index()).modify(|w| {
+            w.set_stall(true);
+        });
+    }
+
+    async fn accept_set_address(&mut self, addr: u8) {
+        trace!("setting addr: {}", addr);
+        critical_section::with(|_| {
+            self.regs.dcfg().modify(|w| {
+                w.set_dad(addr);
+            });
+        });
+
+        // synopsys driver requires accept to be sent after changing address
+        self.accept().await
+    }
+}
+
+/// Translates HAL [EndpointType] into PAC [vals::Eptyp]
+fn to_eptyp(ep_type: EndpointType) -> vals::Eptyp {
+    match ep_type {
+        EndpointType::Control => vals::Eptyp::CONTROL,
+        EndpointType::Isochronous => vals::Eptyp::ISOCHRONOUS,
+        EndpointType::Bulk => vals::Eptyp::BULK,
+        EndpointType::Interrupt => vals::Eptyp::INTERRUPT,
+    }
+}
+
+/// Calculates total allocated FIFO size in words
+fn ep_fifo_size(eps: &[Option<EndpointData>]) -> u16 {
+    eps.iter().map(|ep| ep.map(|ep| ep.fifo_size_words).unwrap_or(0)).sum()
+}
+
+/// Generates IRQ mask for enabled endpoints
+fn ep_irq_mask(eps: &[Option<EndpointData>]) -> u16 {
+    eps.iter().enumerate().fold(
+        0,
+        |mask, (index, ep)| {
+            if ep.is_some() {
+                mask | (1 << index)
+            } else {
+                mask
+            }
+        },
+    )
+}
+
+/// Calculates MPSIZ value for EP0, which uses special values.
+fn ep0_mpsiz(max_packet_size: u16) -> u16 {
+    match max_packet_size {
+        8 => 0b11,
+        16 => 0b10,
+        32 => 0b01,
+        64 => 0b00,
+        other => panic!("Unsupported EP0 size: {}", other),
+    }
+}
+
+/// The number of maximum configurable EPs.
+// TODO: this should at least be configurable, but ideally not a constant.
+// Using OtgInstance::ENDPOINT_COUNT requires feature(const_generic_expr) so just define maximum eps
+pub const MAX_EP_COUNT: usize = 9;
+
+/// USB instance.
+pub struct OtgInstance<'d> {
+    /// The USB peripheral.
+    pub regs: Otg,
+    /// The USB state.
+    pub state: &'d State<{ MAX_EP_COUNT }>,
+    /// FIFO depth in words.
+    pub fifo_depth_words: u16,
+    /// Number of used endpoints.
+    pub endpoint_count: usize,
+    /// The PHY type.
+    pub phy_type: PhyType,
+    /// Extra RX FIFO words needed by some implementations.
+    pub extra_rx_fifo_words: u16,
+    /// Whether to set up late cnak
+    pub quirk_setup_late_cnak: bool,
+    /// Function to calculate TRDT value based on some internal clock speed.
+    pub calculate_trdt_fn: fn(speed: vals::Dspd) -> u8,
+}