1 files changed, 33 insertions, 4 deletions

diff --git a/embassy-rp/src/usb.rs b/embassy-rp/src/usb.rs
index 37d37d6d9..512271ae4 100644
--- a/embassy-rp/src/usb.rs
+++ b/embassy-rp/src/usb.rs
@@ -412,12 +412,41 @@ impl<'d, T: Instance> driver::Bus for Bus<'d, T> {
         .await
     }
 
-    fn endpoint_set_stalled(&mut self, _ep_addr: EndpointAddress, _stalled: bool) {
-        todo!();
+    fn endpoint_set_stalled(&mut self, ep_addr: EndpointAddress, stalled: bool) {
+        let n = ep_addr.index();
+
+        if n == 0 {
+            T::regs().ep_stall_arm().modify(|w| {
+                if ep_addr.is_in() {
+                    w.set_ep0_in(stalled);
+                } else {
+                    w.set_ep0_out(stalled);
+                }
+            });
+        }
+
+        let ctrl = if ep_addr.is_in() {
+            T::dpram().ep_in_buffer_control(n)
+        } else {
+            T::dpram().ep_out_buffer_control(n)
+        };
+
+        ctrl.modify(|w| w.set_stall(stalled));
+
+        let wakers = if ep_addr.is_in() { &EP_IN_WAKERS } else { &EP_OUT_WAKERS };
+        wakers[n].wake();
     }
 
-    fn endpoint_is_stalled(&mut self, _ep_addr: EndpointAddress) -> bool {
-        todo!();
+    fn endpoint_is_stalled(&mut self, ep_addr: EndpointAddress) -> bool {
+        let n = ep_addr.index();
+
+        let ctrl = if ep_addr.is_in() {
+            T::dpram().ep_in_buffer_control(n)
+        } else {
+            T::dpram().ep_out_buffer_control(n)
+        };
+
+        ctrl.read().stall()
     }
 
     fn endpoint_set_enabled(&mut self, ep_addr: EndpointAddress, enabled: bool) {