From 1e726fe405cb8e218398a42bb14ca571e25466b5 Mon Sep 17 00:00:00 2001
From: Arjan Mels <43108771+arjanmels@users.noreply.github.com>
Date: Mon, 8 Feb 2021 05:47:08 +0100
Subject: [PATCH] Fix STM32F1 emergency parser (#21011)

---
 Marlin/src/HAL/STM32F1/HAL.cpp    | 29 +++++++++++++++++++++++++++-
 Marlin/src/HAL/STM32F1/msc_sd.cpp | 32 +++++++++++++++++++++----------
 2 files changed, 50 insertions(+), 11 deletions(-)

diff --git a/Marlin/src/HAL/STM32F1/HAL.cpp b/Marlin/src/HAL/STM32F1/HAL.cpp
index c1e29a843c..2f29b9b0e3 100644
--- a/Marlin/src/HAL/STM32F1/HAL.cpp
+++ b/Marlin/src/HAL/STM32F1/HAL.cpp
@@ -84,7 +84,32 @@
 
 #if defined(SERIAL_USB) && !HAS_SD_HOST_DRIVE
   USBSerial SerialUSB;
-  DefaultSerial MSerial(false, SerialUSB);
+  DefaultSerial MSerial(true, SerialUSB);
+
+  #if ENABLED(EMERGENCY_PARSER)
+    #include "../libmaple/usb/stm32f1/usb_reg_map.h"
+    #include "libmaple/usb_cdcacm.h"
+    // The original callback is not called (no way to retrieve address).
+    // That callback detects a special STM32 reset sequence: this functionality is not essential
+    // as M997 achieves the same.
+    void my_rx_callback(unsigned int, void*) {
+      // max length of 16 is enough to contain all emergency commands
+      uint8 buf[16];
+
+      //rx is usbSerialPart.endpoints[2]
+      uint16 len = usb_get_ep_rx_count(USB_CDCACM_RX_ENDP);
+      uint32 total = usb_cdcacm_data_available();
+
+      if (len == 0 || total == 0 || !WITHIN(total, len, COUNT(buf)))
+        return;
+
+      // cannot get character by character due to bug in composite_cdcacm_peek_ex
+      len = usb_cdcacm_peek(buf, total);
+
+      for (uint32 i = 0; i < len; i++)
+        emergency_parser.update(MSerial.emergency_state, buf[i + total - len]);
+    }
+  #endif
 #endif
 
 uint16_t HAL_adc_result;
@@ -254,6 +279,8 @@ void HAL_init() {
   #endif
   #if HAS_SD_HOST_DRIVE
     MSC_SD_init();
+  #elif BOTH(SERIAL_USB, EMERGENCY_PARSER)
+    usb_cdcacm_set_hooks(USB_CDCACM_HOOK_RX, my_rx_callback);
   #endif
   #if PIN_EXISTS(USB_CONNECT)
     OUT_WRITE(USB_CONNECT_PIN, !USB_CONNECT_INVERTING);  // USB clear connection
diff --git a/Marlin/src/HAL/STM32F1/msc_sd.cpp b/Marlin/src/HAL/STM32F1/msc_sd.cpp
index 548a6dbc57..a916184999 100644
--- a/Marlin/src/HAL/STM32F1/msc_sd.cpp
+++ b/Marlin/src/HAL/STM32F1/msc_sd.cpp
@@ -19,6 +19,7 @@
 
 #include "msc_sd.h"
 #include "SPI.h"
+#include "usb_reg_map.h"
 
 #define PRODUCT_ID 0x29
 
@@ -41,14 +42,27 @@ Serial0Type<USBCompositeSerial> MarlinCompositeSerial(true);
 #endif
 
 #if ENABLED(EMERGENCY_PARSER)
-  void (*real_rx_callback)(void);
 
-  void my_rx_callback(void) {
-    real_rx_callback();
-    int len = MarlinCompositeSerial.available();
-    while (len-- > 0) // >0 because available() may return a negative value
-      emergency_parser.update(MarlinCompositeSerial.emergency_state, MarlinCompositeSerial.peek());
-  }
+// The original callback is not called (no way to retrieve address).
+// That callback detects a special STM32 reset sequence: this functionality is not essential
+// as M997 achieves the same.
+void my_rx_callback(unsigned int, void*) {
+  // max length of 16 is enough to contain all emergency commands
+  uint8 buf[16];
+
+  //rx is usbSerialPart.endpoints[2]
+  uint16 len = usb_get_ep_rx_count(usbSerialPart.endpoints[2].address);
+  uint32 total = composite_cdcacm_data_available();
+
+  if (len == 0 || total == 0 || !WITHIN(total, len, COUNT(buf)))
+    return;
+
+  // cannot get character by character due to bug in composite_cdcacm_peek_ex
+  len = composite_cdcacm_peek(buf, total);
+
+  for (uint32 i = 0; i < len; i++)
+    emergency_parser.update(MarlinCompositeSerial.emergency_state, buf[i+total-len]);
+}
 #endif
 
 void MSC_SD_init() {
@@ -73,9 +87,7 @@ void MSC_SD_init() {
   MarlinCompositeSerial.registerComponent();
   USBComposite.begin();
   #if ENABLED(EMERGENCY_PARSER)
-    //rx is usbSerialPart.endpoints[2]
-    real_rx_callback = usbSerialPart.endpoints[2].callback;
-    usbSerialPart.endpoints[2].callback = my_rx_callback;
+  	composite_cdcacm_set_hooks(USBHID_CDCACM_HOOK_RX, my_rx_callback);
   #endif
 }