From 199729e57a658ee96bb3165fbe3a7f0dea87bd66 Mon Sep 17 00:00:00 2001
From: Ondrej Mikle <ondrej.mikle@gmail.com>
Date: Mon, 6 Dec 2021 22:08:20 +0100
Subject: [PATCH] fix(legacy): prevent handling RebootToBootloader recursing by
 flushing USB write

---
 legacy/firmware/.changelog.d/1985.fixed |  1 +
 legacy/firmware/fsm.c                   |  3 +--
 legacy/firmware/udp.c                   |  2 ++
 legacy/firmware/usb.c                   | 20 ++++++++++++++++++++
 legacy/firmware/usb.h                   |  1 +
 5 files changed, 25 insertions(+), 2 deletions(-)
 create mode 100644 legacy/firmware/.changelog.d/1985.fixed

diff --git a/legacy/firmware/.changelog.d/1985.fixed b/legacy/firmware/.changelog.d/1985.fixed
new file mode 100644
index 0000000000..61ecf6b534
--- /dev/null
+++ b/legacy/firmware/.changelog.d/1985.fixed
@@ -0,0 +1 @@
+Prevent recursing in handling RebootToBootloader by USB flush.
diff --git a/legacy/firmware/fsm.c b/legacy/firmware/fsm.c
index b315cb213c..b744a92cf8 100644
--- a/legacy/firmware/fsm.c
+++ b/legacy/firmware/fsm.c
@@ -360,8 +360,7 @@ void fsm_msgRebootToBootloader(void) {
   oledRefresh();
   fsm_sendSuccess(_("Rebooting"));
   // make sure the outgoing message is sent
-  usbPoll();
-  usbSleep(500);
+  usbFlush(500);
 #if !EMULATOR
   svc_reboot_to_bootloader();
 #else
diff --git a/legacy/firmware/udp.c b/legacy/firmware/udp.c
index 4a8918f376..dd56ec934a 100644
--- a/legacy/firmware/udp.c
+++ b/legacy/firmware/udp.c
@@ -70,3 +70,5 @@ char usbTiny(char set) {
   tiny = set;
   return old;
 }
+
+void usbFlush(uint32_t millis) { usbSleep(millis); }
diff --git a/legacy/firmware/usb.c b/legacy/firmware/usb.c
index 1b583952fe..8b98e2a36f 100644
--- a/legacy/firmware/usb.c
+++ b/legacy/firmware/usb.c
@@ -457,3 +457,23 @@ void usbSleep(uint32_t millis) {
     }
   }
 }
+
+void usbFlush(uint32_t millis) {
+  if (usbd_dev == NULL) {
+    return;
+  }
+
+  static const uint8_t *data;
+  data = msg_out_data();
+  if (data) {
+    while (usbd_ep_write_packet(usbd_dev, ENDPOINT_ADDRESS_MAIN_IN, data,
+                                USB_PACKET_SIZE) != USB_PACKET_SIZE) {
+    }
+  }
+
+  uint32_t start = timer_ms();
+
+  while ((timer_ms() - start) < millis) {
+    asm("nop");
+  }
+}
diff --git a/legacy/firmware/usb.h b/legacy/firmware/usb.h
index f1fc55837b..8df36606de 100644
--- a/legacy/firmware/usb.h
+++ b/legacy/firmware/usb.h
@@ -27,5 +27,6 @@ void usbPoll(void);
 void usbReconnect(void);
 char usbTiny(char set);
 void usbSleep(uint32_t millis);
+void usbFlush(uint32_t millis);
 
 #endif