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) {