5 files changed, 44 insertions, 38 deletions

diff --git a/embassy-nrf/src/usb.rs b/embassy-nrf/src/usb.rs
index ed4d5cf35..6be4fec8c 100644
--- a/embassy-nrf/src/usb.rs
+++ b/embassy-nrf/src/usb.rs
@@ -391,11 +391,6 @@ impl<'d, T: Instance, P: UsbSupply> driver::Bus for Bus<'d, T, P> {
         .await
     }
 
-    #[inline]
-    fn set_address(&mut self, _addr: u8) {
-        // Nothing to do, the peripheral handles this.
-    }
-
     fn endpoint_set_stalled(&mut self, ep_addr: EndpointAddress, stalled: bool) {
         let regs = T::regs();
         unsafe {
@@ -841,6 +836,11 @@ impl<'d, T: Instance> driver::ControlPipe for ControlPipe<'d, T> {
         let regs = T::regs();
         regs.tasks_ep0stall.write(|w| w.tasks_ep0stall().bit(true));
     }
+
+    async fn accept_set_address(&mut self, _addr: u8) {
+        self.accept().await;
+        // Nothing to do, the peripheral handles this.
+    }
 }
 
 fn dma_start() {
diff --git a/embassy-rp/src/usb.rs b/embassy-rp/src/usb.rs
index 8361736a9..2710c4ad9 100644
--- a/embassy-rp/src/usb.rs
+++ b/embassy-rp/src/usb.rs
@@ -406,13 +406,6 @@ impl<'d, T: Instance> driver::Bus for Bus<'d, T> {
         .await
     }
 
-    #[inline]
-    fn set_address(&mut self, addr: u8) {
-        let regs = T::regs();
-        trace!("setting addr: {}", addr);
-        unsafe { regs.addr_endp().write(|w| w.set_address(addr)) }
-    }
-
     fn endpoint_set_stalled(&mut self, _ep_addr: EndpointAddress, _stalled: bool) {
         todo!();
     }
@@ -812,4 +805,12 @@ impl<'d, T: Instance> driver::ControlPipe for ControlPipe<'d, T> {
             T::dpram().ep_in_buffer_control(0).write(|w| w.set_stall(true));
         }
     }
+
+    async fn accept_set_address(&mut self, addr: u8) {
+        self.accept().await;
+
+        let regs = T::regs();
+        trace!("setting addr: {}", addr);
+        unsafe { regs.addr_endp().write(|w| w.set_address(addr)) }
+    }
 }
diff --git a/embassy-stm32/src/usb/usb.rs b/embassy-stm32/src/usb/usb.rs
index cba4915c1..062c7ef77 100644
--- a/embassy-stm32/src/usb/usb.rs
+++ b/embassy-stm32/src/usb/usb.rs
@@ -489,18 +489,6 @@ impl<'d, T: Instance> driver::Bus for Bus<'d, T> {
         .await
     }
 
-    #[inline]
-    fn set_address(&mut self, addr: u8) {
-        let regs = T::regs();
-        trace!("setting addr: {}", addr);
-        unsafe {
-            regs.daddr().write(|w| {
-                w.set_ef(true);
-                w.set_add(addr);
-            })
-        }
-    }
-
     fn endpoint_set_stalled(&mut self, ep_addr: EndpointAddress, stalled: bool) {
         // This can race, so do a retry loop.
         let reg = T::regs().epr(ep_addr.index() as _);
@@ -1017,4 +1005,17 @@ impl<'d, T: Instance> driver::ControlPipe for ControlPipe<'d, T> {
             });
         }
     }
+
+    async fn accept_set_address(&mut self, addr: u8) {
+        self.accept().await;
+
+        let regs = T::regs();
+        trace!("setting addr: {}", addr);
+        unsafe {
+            regs.daddr().write(|w| {
+                w.set_ef(true);
+                w.set_add(addr);
+            })
+        }
+    }
 }
diff --git a/embassy-usb-driver/src/lib.rs b/embassy-usb-driver/src/lib.rs
index d7238dc7d..64d647351 100644
--- a/embassy-usb-driver/src/lib.rs
+++ b/embassy-usb-driver/src/lib.rs
@@ -164,9 +164,6 @@ pub trait Bus {
 
     async fn poll(&mut self) -> Event;
 
-    /// Sets the device USB address to `addr`.
-    fn set_address(&mut self, addr: u8);
-
     /// Enables or disables an endpoint.
     fn endpoint_set_enabled(&mut self, ep_addr: EndpointAddress, enabled: bool);
 
@@ -306,6 +303,12 @@ pub trait ControlPipe {
     ///
     /// Sets a STALL condition on the pipe to indicate an error.
     async fn reject(&mut self);
+
+    /// Accept SET_ADDRESS control and change bus address.
+    ///
+    /// For most drivers this function should firstly call `accept()` and then change the bus address.
+    /// However, there are peripherals (Synopsys USB OTG) that have reverse order.
+    async fn accept_set_address(&mut self, addr: u8);
 }
 
 pub trait EndpointIn: Endpoint {
diff --git a/embassy-usb/src/lib.rs b/embassy-usb/src/lib.rs
index 661b84119..096e8b07a 100644
--- a/embassy-usb/src/lib.rs
+++ b/embassy-usb/src/lib.rs
@@ -122,10 +122,9 @@ struct Inner<'d, D: Driver<'d>> {
 
     /// Our device address, or 0 if none.
     address: u8,
-    /// When receiving a set addr control request, we have to apply it AFTER we've
-    /// finished handling the control request, as the status stage still has to be
-    /// handled with addr 0.
-    /// If true, do a set_addr after finishing the current control req.
+    /// SET_ADDRESS requests have special handling depending on the driver.
+    /// This flag indicates that requests must be handled by `ControlPipe::accept_set_address()`
+    /// instead of regular `accept()`.
     set_address_pending: bool,
 
     interfaces: Vec<Interface<'d>, MAX_INTERFACE_COUNT>,
@@ -254,11 +253,6 @@ impl<'d, D: Driver<'d>> UsbDevice<'d, D> {
             Direction::In => self.handle_control_in(req).await,
             Direction::Out => self.handle_control_out(req).await,
         }
-
-        if self.inner.set_address_pending {
-            self.inner.bus.set_address(self.inner.address);
-            self.inner.set_address_pending = false;
-        }
     }
 
     async fn handle_control_in(&mut self, req: Request) {
@@ -328,7 +322,14 @@ impl<'d, D: Driver<'d>> UsbDevice<'d, D> {
         trace!("  control out data: {:02x?}", data);
 
         match self.inner.handle_control_out(req, data) {
-            OutResponse::Accepted => self.control.accept().await,
+            OutResponse::Accepted => {
+                if self.inner.set_address_pending {
+                    self.control.accept_set_address(self.inner.address).await;
+                    self.inner.set_address_pending = false;
+                } else {
+                    self.control.accept().await
+                }
+            }
             OutResponse::Rejected => self.control.reject().await,
         }
     }
@@ -655,7 +656,7 @@ impl<'d, D: Driver<'d>> Inner<'d, D> {
                         buf[1] = descriptor_type::STRING;
                         let mut pos = 2;
                         for c in s.encode_utf16() {
-                            if pos >= buf.len() {
+                            if pos + 2 >= buf.len() {
                                 panic!("control buffer too small");
                             }