3 files changed, 36 insertions, 10 deletions

diff --git a/embassy-stm32/src/rcc/f4f7.rs b/embassy-stm32/src/rcc/f4f7.rs
index 2e4f95722..d507a6fd4 100644
--- a/embassy-stm32/src/rcc/f4f7.rs
+++ b/embassy-stm32/src/rcc/f4f7.rs
@@ -113,6 +113,14 @@ pub(crate) unsafe fn init(config: Config) {
         while !PWR.csr1().read().odswrdy() {}
     }
 
+    #[cfg(any(stm32f401, stm32f410, stm32f411, stm32f412, stm32f413, stm32f423))]
+    {
+        use crate::pac::pwr::vals::Vos;
+        use crate::pac::PWR;
+
+        PWR.cr1().modify(|w| w.set_vos(Vos::SCALE1));
+    }
+
     // Configure HSI
     let hsi = match config.hsi {
         false => {
diff --git a/embassy-stm32/src/usb_otg/usb.rs b/embassy-stm32/src/usb_otg/usb.rs
index e45e4ac43..d90f83433 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()),
             }
@@ -911,11 +926,9 @@ impl<'d, T: Instance> embassy_usb_driver::Bus for Bus<'d, T> {
                 trace!("enumdne");
 
                 let speed = r.dsts().read().enumspd();
-                trace!("  speed={}", speed.to_bits());
-
-                r.gusbcfg().modify(|w| {
-                    w.set_trdt(calculate_trdt(speed, T::frequency()));
-                });
+                let trdt = calculate_trdt(speed, T::frequency());
+                trace!("  speed={} trdt={}", speed.to_bits(), trdt);
+                r.gusbcfg().modify(|w| w.set_trdt(trdt));
 
                 r.gintsts().write(|w| w.set_enumdne(true)); // clear
                 Self::restore_irqs();
@@ -1314,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 {
diff --git a/embassy-usb/src/lib.rs b/embassy-usb/src/lib.rs
index 88d88cad7..9fc1e3320 100644
--- a/embassy-usb/src/lib.rs
+++ b/embassy-usb/src/lib.rs
@@ -406,6 +406,16 @@ impl<'d, D: Driver<'d>> UsbDevice<'d, D> {
         let max_packet_size = self.control.max_packet_size();
         let mut total = 0;
 
+        if req_length > self.control_buf.len() {
+            warn!(
+                "got CONTROL OUT with length {} higher than the control_buf len {}, rejecting.",
+                req_length,
+                self.control_buf.len()
+            );
+            self.control.reject().await;
+            return;
+        }
+
         let chunks = self.control_buf[..req_length].chunks_mut(max_packet_size);
         for (first, last, chunk) in first_last(chunks) {
             let size = match self.control.data_out(chunk, first, last).await {