diff --git a/embassy-stm32/src/usb_otg/usb.rs b/embassy-stm32/src/usb_otg/usb.rs
index 26a14ff0..d90f8343 100644
--- a/embassy-stm32/src/usb_otg/usb.rs
+++ b/embassy-stm32/src/usb_otg/usb.rs
@@ -40,6 +40,7 @@ impl<T: Instance> interrupt::typelevel::Handler<T::Interrupt> for InterruptHandl
         // 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;
 
@@ -51,6 +52,15 @@ impl<T: Instance> interrupt::typelevel::Handler<T::Interrupt> for InterruptHandl
                     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() };
@@ -96,6 +106,11 @@ impl<T: Instance> interrupt::typelevel::Handler<T::Interrupt> for InterruptHandl
                 }
                 vals::Pktstsd::SETUP_DATA_DONE => {
                     trace!("SETUP_DATA_DONE ep={}", ep_num);
+
+                    // Clear NAK to indicate we are ready to receive more data
+                    T::regs().doepctl(ep_num).modify(|w| {
+                        w.set_cnak(true);
+                    });
                 }
                 x => trace!("unknown PKTSTS: {}", x.to_bits()),
             }
@@ -1312,11 +1327,6 @@ impl<'d, T: Instance> embassy_usb_driver::ControlPipe for ControlPipe<'d, T> {
                     w.set_rxdpid_stupcnt(1);
                 });
 
-                // Clear NAK to indicate we are ready to receive more data
-                T::regs().doepctl(self.ep_out.info.addr.index()).modify(|w| {
-                    w.set_cnak(true);
-                });
-
                 trace!("SETUP received: {:?}", data);
                 Poll::Ready(data)
             } else {