1 files changed, 31 insertions, 12 deletions

diff --git a/embassy-usb-synopsys-otg/src/lib.rs b/embassy-usb-synopsys-otg/src/lib.rs
index fc4428b54..7fc142a4e 100644
--- a/embassy-usb-synopsys-otg/src/lib.rs
+++ b/embassy-usb-synopsys-otg/src/lib.rs
@@ -345,6 +345,7 @@ impl<'d, const MAX_EP_COUNT: usize> Driver<'d, MAX_EP_COUNT> {
     fn alloc_endpoint<D: Dir>(
         &mut self,
         ep_type: EndpointType,
+        ep_addr: Option<EndpointAddress>,
         max_packet_size: u16,
         interval_ms: u8,
     ) -> Result<Endpoint<'d, D>, EndpointAllocError> {
@@ -379,15 +380,31 @@ impl<'d, const MAX_EP_COUNT: usize> Driver<'d, MAX_EP_COUNT> {
             Direction::In => &mut self.ep_in[..self.instance.endpoint_count],
         };
 
-        // 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()
+        // Find endpoint slot
+        let slot = if let Some(addr) = ep_addr {
+            // Use the specified endpoint address
+            let requested_index = addr.index();
+            if requested_index >= self.instance.endpoint_count {
+                return Err(EndpointAllocError);
             }
-        });
+            if requested_index == 0 && ep_type != EndpointType::Control {
+                return Err(EndpointAllocError); // EP0 is reserved for control
+            }
+            if eps[requested_index].is_some() {
+                return Err(EndpointAllocError); // Already allocated
+            }
+            Some((requested_index, &mut eps[requested_index]))
+        } else {
+            // Find any free endpoint 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)) => {
@@ -438,27 +455,29 @@ impl<'d, const MAX_EP_COUNT: usize> embassy_usb_driver::Driver<'d> for Driver<'d
     fn alloc_endpoint_in(
         &mut self,
         ep_type: EndpointType,
+        ep_addr: Option<EndpointAddress>,
         max_packet_size: u16,
         interval_ms: u8,
     ) -> Result<Self::EndpointIn, EndpointAllocError> {
-        self.alloc_endpoint(ep_type, max_packet_size, interval_ms)
+        self.alloc_endpoint(ep_type, ep_addr, max_packet_size, interval_ms)
     }
 
     fn alloc_endpoint_out(
         &mut self,
         ep_type: EndpointType,
+        ep_addr: Option<EndpointAddress>,
         max_packet_size: u16,
         interval_ms: u8,
     ) -> Result<Self::EndpointOut, EndpointAllocError> {
-        self.alloc_endpoint(ep_type, max_packet_size, interval_ms)
+        self.alloc_endpoint(ep_type, ep_addr, 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)
+            .alloc_endpoint(EndpointType::Control, None, control_max_packet_size, 0)
             .unwrap();
         let ep_in = self
-            .alloc_endpoint(EndpointType::Control, control_max_packet_size, 0)
+            .alloc_endpoint(EndpointType::Control, None, control_max_packet_size, 0)
             .unwrap();
         assert_eq!(ep_out.info.addr.index(), 0);
         assert_eq!(ep_in.info.addr.index(), 0);