From 28fb492c402c74add5c650b9d65687816585c923 Mon Sep 17 00:00:00 2001
From: Dario Nieuwenhuis <dirbaio@dirbaio.net>
Date: Tue, 27 Jun 2023 00:42:24 +0200
Subject: [PATCH] stm32/otg: flush fifos on reconfigure and on ep disable.

---
 embassy-stm32/src/usb_otg/usb.rs | 27 ++++++++++++++++++++++++++-
 1 file changed, 26 insertions(+), 1 deletion(-)

diff --git a/embassy-stm32/src/usb_otg/usb.rs b/embassy-stm32/src/usb_otg/usb.rs
index 8af5c7bd..532157e6 100644
--- a/embassy-stm32/src/usb_otg/usb.rs
+++ b/embassy-stm32/src/usb_otg/usb.rs
@@ -540,6 +540,19 @@ impl<'d, T: Instance> Bus<'d, T> {
             fifo_top <= T::FIFO_DEPTH_WORDS,
             "FIFO allocations exceeded maximum capacity"
         );
+
+        // Flush fifos
+        r.grstctl().write(|w| {
+            w.set_rxfflsh(true);
+            w.set_txfflsh(true);
+            w.set_txfnum(0x10);
+        });
+        loop {
+            let x = r.grstctl().read();
+            if !x.rxfflsh() && !x.txfflsh() {
+                break;
+            }
+        }
     }
 
     fn configure_endpoints(&mut self) {
@@ -744,7 +757,19 @@ impl<'d, T: Instance> embassy_usb_driver::Bus for Bus<'d, T> {
 
                     r.doepctl(ep_addr.index()).modify(|w| {
                         w.set_usbaep(enabled);
-                    })
+                    });
+
+                    // Flush tx fifo
+                    r.grstctl().write(|w| {
+                        w.set_txfflsh(true);
+                        w.set_txfnum(ep_addr.index() as _);
+                    });
+                    loop {
+                        let x = r.grstctl().read();
+                        if !x.txfflsh() {
+                            break;
+                        }
+                    }
                 });
 
                 // Wake `Endpoint::wait_enabled()`