From d6955f25b278904e61b296201a5bee9f222cd4ca Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Eduardo=20Jos=C3=A9=20Tagle?= <ejtagle@hotmail.com>
Date: Wed, 3 Oct 2018 02:47:27 -0300
Subject: [PATCH] Expand serial support in DUE/AVR hals exploiting the
 templated MarlinSerial classes (#11988)

---
 Marlin/src/HAL/HAL_AVR/HAL.h                  |   22 +-
 Marlin/src/HAL/HAL_AVR/MarlinSerial.cpp       |   27 +-
 Marlin/src/HAL/HAL_AVR/MarlinSerial.h         |   24 +-
 Marlin/src/HAL/HAL_DUE/HAL.cpp                |    1 +
 Marlin/src/HAL/HAL_DUE/HAL.h                  |   22 +-
 Marlin/src/HAL/HAL_DUE/HAL_timers_Due.cpp     |    2 +-
 .../src/HAL/HAL_DUE/InterruptVectors_Due.cpp  |    1 +
 .../src/HAL/HAL_DUE/MarlinSerialUSB_Due.cpp   |    2 +-
 Marlin/src/HAL/HAL_DUE/MarlinSerialUSB_Due.h  |    2 +-
 Marlin/src/HAL/HAL_DUE/MarlinSerial_Due.cpp   | 1054 +++++++++--------
 Marlin/src/HAL/HAL_DUE/MarlinSerial_Due.h     |   49 +-
 Marlin/src/HAL/HAL_DUE/Tone.cpp               |    1 +
 Marlin/src/gcode/control/M111.cpp             |    8 +-
 Marlin/src/gcode/queue.cpp                    |    4 +-
 14 files changed, 661 insertions(+), 558 deletions(-)

diff --git a/Marlin/src/HAL/HAL_AVR/HAL.h b/Marlin/src/HAL/HAL_AVR/HAL.h
index 5a7f41ed2d..fcc87c88b6 100644
--- a/Marlin/src/HAL/HAL_AVR/HAL.h
+++ b/Marlin/src/HAL/HAL_AVR/HAL.h
@@ -79,16 +79,32 @@ typedef int8_t pin_t;
 
 //extern uint8_t MCUSR;
 
-#define NUM_SERIAL 1
-
+// Serial ports
 #ifdef USBCON
   #if ENABLED(BLUETOOTH)
     #define MYSERIAL0 bluetoothSerial
   #else
     #define MYSERIAL0 Serial
   #endif
+  #define NUM_SERIAL 1
 #else
-  #define MYSERIAL0 customizedSerial
+  #if !WITHIN(SERIAL_PORT, -1, 3)
+    #error "SERIAL_PORT must be from -1 to 3"
+  #endif
+
+  #define MYSERIAL0 customizedSerial1
+
+  #ifdef SERIAL_PORT_2
+    #if !WITHIN(SERIAL_PORT_2, -1, 3)
+      #error "SERIAL_PORT_2 must be from -1 to 3"
+    #elif SERIAL_PORT_2 == SERIAL_PORT
+      #error "SERIAL_PORT_2 must be different than SERIAL_PORT"
+    #endif
+    #define NUM_SERIAL 2
+    #define MYSERIAL1 customizedSerial2
+  #else
+    #define NUM_SERIAL 1
+  #endif
 #endif
 
 // --------------------------------------------------------------------------
diff --git a/Marlin/src/HAL/HAL_AVR/MarlinSerial.cpp b/Marlin/src/HAL/HAL_AVR/MarlinSerial.cpp
index 32a6cc340b..c0181296ea 100644
--- a/Marlin/src/HAL/HAL_AVR/MarlinSerial.cpp
+++ b/Marlin/src/HAL/HAL_AVR/MarlinSerial.cpp
@@ -705,18 +705,37 @@
 
   // Hookup ISR handlers
   ISR(SERIAL_REGNAME(USART,SERIAL_PORT,_RX_vect)) {
-    MarlinSerial<MarlinSerialCfg>::store_rxd_char();
+    MarlinSerial<MarlinSerialCfg1>::store_rxd_char();
   }
 
   ISR(SERIAL_REGNAME(USART,SERIAL_PORT,_UDRE_vect)) {
-    MarlinSerial<MarlinSerialCfg>::_tx_udr_empty_irq();
+    MarlinSerial<MarlinSerialCfg1>::_tx_udr_empty_irq();
   }
 
   // Preinstantiate
-  template class MarlinSerial<MarlinSerialCfg>;
+  template class MarlinSerial<MarlinSerialCfg1>;
 
   // Instantiate
-  MarlinSerial<MarlinSerialCfg> customizedSerial;
+  MarlinSerial<MarlinSerialCfg1> customizedSerial1;
+
+  #ifdef SERIAL_PORT_2
+
+    // Hookup ISR handlers
+    ISR(SERIAL_REGNAME(USART,SERIAL_PORT_2,_RX_vect)) {
+      MarlinSerial<MarlinSerialCfg2>::store_rxd_char();
+    }
+
+    ISR(SERIAL_REGNAME(USART,SERIAL_PORT_2,_UDRE_vect)) {
+      MarlinSerial<MarlinSerialCfg2>::_tx_udr_empty_irq();
+    }
+
+    // Preinstantiate
+    template class MarlinSerial<MarlinSerialCfg2>;
+
+    // Instantiate
+    MarlinSerial<MarlinSerialCfg2> customizedSerial2;
+
+  #endif
 
 #endif // !USBCON && (UBRRH || UBRR0H || UBRR1H || UBRR2H || UBRR3H)
 
diff --git a/Marlin/src/HAL/HAL_AVR/MarlinSerial.h b/Marlin/src/HAL/HAL_AVR/MarlinSerial.h
index b378567b6e..399bed97cb 100644
--- a/Marlin/src/HAL/HAL_AVR/MarlinSerial.h
+++ b/Marlin/src/HAL/HAL_AVR/MarlinSerial.h
@@ -256,7 +256,7 @@
   };
 
   // Serial port configuration
-  struct MarlinSerialCfg {
+  struct MarlinSerialCfg1 {
     static constexpr int PORT               = SERIAL_PORT;
     static constexpr unsigned int RX_SIZE   = RX_BUFFER_SIZE;
     static constexpr unsigned int TX_SIZE   = TX_BUFFER_SIZE;
@@ -268,7 +268,27 @@
     static constexpr bool MAX_RX_QUEUED     = bSERIAL_STATS_MAX_RX_QUEUED;
   };
 
-  extern MarlinSerial<MarlinSerialCfg> customizedSerial;
+  extern MarlinSerial<MarlinSerialCfg1> customizedSerial1;
+
+  #ifdef SERIAL_PORT_2
+
+    // Serial port configuration
+    struct MarlinSerialCfg2 {
+      static constexpr int PORT               = SERIAL_PORT_2;
+      static constexpr unsigned int RX_SIZE   = RX_BUFFER_SIZE;
+      static constexpr unsigned int TX_SIZE   = TX_BUFFER_SIZE;
+      static constexpr bool XONOFF            = bSERIAL_XON_XOFF;
+      static constexpr bool EMERGENCYPARSER   = bEMERGENCY_PARSER;
+      static constexpr bool DROPPED_RX        = bSERIAL_STATS_DROPPED_RX;
+      static constexpr bool RX_OVERRUNS       = bSERIAL_STATS_RX_BUFFER_OVERRUNS;
+      static constexpr bool RX_FRAMING_ERRORS = bSERIAL_STATS_RX_FRAMING_ERRORS;
+      static constexpr bool MAX_RX_QUEUED     = bSERIAL_STATS_MAX_RX_QUEUED;
+    };
+
+    extern MarlinSerial<MarlinSerialCfg2> customizedSerial2;
+
+  #endif
+
 
 #endif // !USBCON
 
diff --git a/Marlin/src/HAL/HAL_DUE/HAL.cpp b/Marlin/src/HAL/HAL_DUE/HAL.cpp
index 9d932d94db..cc2db0eb26 100644
--- a/Marlin/src/HAL/HAL_DUE/HAL.cpp
+++ b/Marlin/src/HAL/HAL_DUE/HAL.cpp
@@ -29,6 +29,7 @@
 // Includes
 // --------------------------------------------------------------------------
 
+#include "../../inc/MarlinConfig.h"
 #include "HAL.h"
 
 #include <Wire.h>
diff --git a/Marlin/src/HAL/HAL_DUE/HAL.h b/Marlin/src/HAL/HAL_DUE/HAL.h
index b5d1d4580f..8e7b4490d3 100644
--- a/Marlin/src/HAL/HAL_DUE/HAL.h
+++ b/Marlin/src/HAL/HAL_DUE/HAL.h
@@ -41,9 +41,25 @@
 #include "watchdog_Due.h"
 #include "HAL_timers_Due.h"
 
-#define NUM_SERIAL 1
-// Required before the include or compilation fails
-#define MYSERIAL0 customizedSerial
+// Serial ports
+#if !WITHIN(SERIAL_PORT, -1, 3)
+  #error "SERIAL_PORT must be from -1 to 3"
+#endif
+
+// MYSERIAL0 required before MarlinSerial includes!
+#define MYSERIAL0 customizedSerial1
+
+#ifdef SERIAL_PORT_2
+  #if !WITHIN(SERIAL_PORT_2, -1, 3)
+    #error "SERIAL_PORT_2 must be from -1 to 3"
+  #elif SERIAL_PORT_2 == SERIAL_PORT
+    #error "SERIAL_PORT_2 must be different than SERIAL_PORT"
+  #endif
+  #define NUM_SERIAL 2
+  #define MYSERIAL1 customizedSerial2
+#else
+  #define NUM_SERIAL 1
+#endif
 
 #include "MarlinSerial_Due.h"
 #include "MarlinSerialUSB_Due.h"
diff --git a/Marlin/src/HAL/HAL_DUE/HAL_timers_Due.cpp b/Marlin/src/HAL/HAL_DUE/HAL_timers_Due.cpp
index eb23692b44..f5f74c0ce2 100644
--- a/Marlin/src/HAL/HAL_DUE/HAL_timers_Due.cpp
+++ b/Marlin/src/HAL/HAL_DUE/HAL_timers_Due.cpp
@@ -31,7 +31,7 @@
 // --------------------------------------------------------------------------
 // Includes
 // --------------------------------------------------------------------------
-
+#include "../../inc/MarlinConfig.h"
 #include "HAL.h"
 
 #include "HAL_timers_Due.h"
diff --git a/Marlin/src/HAL/HAL_DUE/InterruptVectors_Due.cpp b/Marlin/src/HAL/HAL_DUE/InterruptVectors_Due.cpp
index 9b226a60da..97aefc98d1 100644
--- a/Marlin/src/HAL/HAL_DUE/InterruptVectors_Due.cpp
+++ b/Marlin/src/HAL/HAL_DUE/InterruptVectors_Due.cpp
@@ -32,6 +32,7 @@
  */
 #ifdef ARDUINO_ARCH_SAM
 
+#include "../../inc/MarlinConfig.h"
 #include "HAL.h"
 #include "InterruptVectors_Due.h"
 
diff --git a/Marlin/src/HAL/HAL_DUE/MarlinSerialUSB_Due.cpp b/Marlin/src/HAL/HAL_DUE/MarlinSerialUSB_Due.cpp
index eed4c73999..b0a00840ad 100644
--- a/Marlin/src/HAL/HAL_DUE/MarlinSerialUSB_Due.cpp
+++ b/Marlin/src/HAL/HAL_DUE/MarlinSerialUSB_Due.cpp
@@ -285,7 +285,7 @@ void MarlinSerialUSB::printFloat(double number, uint8_t digits) {
 }
 
 // Preinstantiate
-MarlinSerialUSB customizedSerial;
+MarlinSerialUSB customizedSerial1;
 
 #endif // SERIAL_PORT == -1
 
diff --git a/Marlin/src/HAL/HAL_DUE/MarlinSerialUSB_Due.h b/Marlin/src/HAL/HAL_DUE/MarlinSerialUSB_Due.h
index 925c322cfd..627f3a9616 100644
--- a/Marlin/src/HAL/HAL_DUE/MarlinSerialUSB_Due.h
+++ b/Marlin/src/HAL/HAL_DUE/MarlinSerialUSB_Due.h
@@ -89,7 +89,7 @@ private:
   static void printFloat(double, uint8_t);
 };
 
-extern MarlinSerialUSB customizedSerial;
+extern MarlinSerialUSB customizedSerial1;
 
 #endif // SERIAL_PORT == -1
 #endif // MARLINSERIAL_DUE_H
diff --git a/Marlin/src/HAL/HAL_DUE/MarlinSerial_Due.cpp b/Marlin/src/HAL/HAL_DUE/MarlinSerial_Due.cpp
index 7efa85e050..853cc60f36 100644
--- a/Marlin/src/HAL/HAL_DUE/MarlinSerial_Due.cpp
+++ b/Marlin/src/HAL/HAL_DUE/MarlinSerial_Due.cpp
@@ -29,345 +29,311 @@
 
 #include "../../inc/MarlinConfig.h"
 
-// If not using the USB port as serial port
-#if SERIAL_PORT >= 0
+#include "MarlinSerial_Due.h"
+#include "InterruptVectors_Due.h"
+#include "../../Marlin.h"
 
-  #include "MarlinSerial_Due.h"
-  #include "InterruptVectors_Due.h"
-  #include "../../Marlin.h"
+template<typename Cfg> typename MarlinSerial<Cfg>::ring_buffer_r MarlinSerial<Cfg>::rx_buffer = { 0 };
+template<typename Cfg> typename MarlinSerial<Cfg>::ring_buffer_t MarlinSerial<Cfg>::tx_buffer = { 0 };
+template<typename Cfg> bool     MarlinSerial<Cfg>::_written = false;
+template<typename Cfg> uint8_t  MarlinSerial<Cfg>::xon_xoff_state = MarlinSerial<Cfg>::XON_XOFF_CHAR_SENT | MarlinSerial<Cfg>::XON_CHAR;
+template<typename Cfg> uint8_t  MarlinSerial<Cfg>::rx_dropped_bytes = 0;
+template<typename Cfg> uint8_t  MarlinSerial<Cfg>::rx_buffer_overruns = 0;
+template<typename Cfg> uint8_t  MarlinSerial<Cfg>::rx_framing_errors = 0;
+template<typename Cfg> typename MarlinSerial<Cfg>::ring_buffer_pos_t MarlinSerial<Cfg>::rx_max_enqueued = 0;
 
-  template<typename Cfg> typename MarlinSerial<Cfg>::ring_buffer_r MarlinSerial<Cfg>::rx_buffer = { 0 };
-  template<typename Cfg> typename MarlinSerial<Cfg>::ring_buffer_t MarlinSerial<Cfg>::tx_buffer = { 0 };
-  template<typename Cfg> bool     MarlinSerial<Cfg>::_written = false;
-  template<typename Cfg> uint8_t  MarlinSerial<Cfg>::xon_xoff_state = MarlinSerial<Cfg>::XON_XOFF_CHAR_SENT | MarlinSerial<Cfg>::XON_CHAR;
-  template<typename Cfg> uint8_t  MarlinSerial<Cfg>::rx_dropped_bytes = 0;
-  template<typename Cfg> uint8_t  MarlinSerial<Cfg>::rx_buffer_overruns = 0;
-  template<typename Cfg> uint8_t  MarlinSerial<Cfg>::rx_framing_errors = 0;
-  template<typename Cfg> typename MarlinSerial<Cfg>::ring_buffer_pos_t MarlinSerial<Cfg>::rx_max_enqueued = 0;
+// A SW memory barrier, to ensure GCC does not overoptimize loops
+#define sw_barrier() asm volatile("": : :"memory");
 
-  // A SW memory barrier, to ensure GCC does not overoptimize loops
-  #define sw_barrier() asm volatile("": : :"memory");
+#include "../../feature/emergency_parser.h"
 
-  #include "../../feature/emergency_parser.h"
+// (called with RX interrupts disabled)
+template<typename Cfg>
+FORCE_INLINE void MarlinSerial<Cfg>::store_rxd_char() {
 
-  // (called with RX interrupts disabled)
-  template<typename Cfg>
-  FORCE_INLINE void MarlinSerial<Cfg>::store_rxd_char() {
+  static EmergencyParser::State emergency_state; // = EP_RESET
 
-    static EmergencyParser::State emergency_state; // = EP_RESET
+  // Get the tail - Nothing can alter its value while we are at this ISR
+  const ring_buffer_pos_t t = rx_buffer.tail;
 
-    // Get the tail - Nothing can alter its value while we are at this ISR
-    const ring_buffer_pos_t t = rx_buffer.tail;
+  // Get the head pointer
+  ring_buffer_pos_t h = rx_buffer.head;
 
-    // Get the head pointer
-    ring_buffer_pos_t h = rx_buffer.head;
+  // Get the next element
+  ring_buffer_pos_t i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
 
-    // Get the next element
-    ring_buffer_pos_t i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
+  // Read the character from the USART
+  uint8_t c = HWUART->UART_RHR;
 
-    // Read the character from the USART
-    uint8_t c = HWUART->UART_RHR;
+  if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c);
 
-    if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c);
+  // If the character is to be stored at the index just before the tail
+  // (such that the head would advance to the current tail), the RX FIFO is
+  // full, so don't write the character or advance the head.
+  if (i != t) {
+    rx_buffer.buffer[h] = c;
+    h = i;
+  }
+  else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
+    --rx_dropped_bytes;
 
-    // If the character is to be stored at the index just before the tail
-    // (such that the head would advance to the current tail), the RX FIFO is
-    // full, so don't write the character or advance the head.
-    if (i != t) {
-      rx_buffer.buffer[h] = c;
-      h = i;
-    }
-    else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
-      --rx_dropped_bytes;
+  const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
+  // Calculate count of bytes stored into the RX buffer
 
-    const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
-    // Calculate count of bytes stored into the RX buffer
+  // Keep track of the maximum count of enqueued bytes
+  if (Cfg::MAX_RX_QUEUED) NOLESS(rx_max_enqueued, rx_count);
 
-    // Keep track of the maximum count of enqueued bytes
-    if (Cfg::MAX_RX_QUEUED) NOLESS(rx_max_enqueued, rx_count);
+  if (Cfg::XONOFF) {
+    // If the last char that was sent was an XON
+    if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XON_CHAR) {
 
-    if (Cfg::XONOFF) {
-      // If the last char that was sent was an XON
-      if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XON_CHAR) {
+      // Bytes stored into the RX buffer
+      const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
 
-        // Bytes stored into the RX buffer
-        const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
+      // If over 12.5% of RX buffer capacity, send XOFF before running out of
+      // RX buffer space .. 325 bytes @ 250kbits/s needed to let the host react
+      // and stop sending bytes. This translates to 13mS propagation time.
+      if (rx_count >= (Cfg::RX_SIZE) / 8) {
 
-        // If over 12.5% of RX buffer capacity, send XOFF before running out of
-        // RX buffer space .. 325 bytes @ 250kbits/s needed to let the host react
-        // and stop sending bytes. This translates to 13mS propagation time.
-        if (rx_count >= (Cfg::RX_SIZE) / 8) {
+        // At this point, definitely no TX interrupt was executing, since the TX isr can't be preempted.
+        // Don't enable the TX interrupt here as a means to trigger the XOFF char, because if it happens
+        // to be in the middle of trying to disable the RX interrupt in the main program, eventually the
+        // enabling of the TX interrupt could be undone. The ONLY reliable thing this can do to ensure
+        // the sending of the XOFF char is to send it HERE AND NOW.
 
-          // At this point, definitely no TX interrupt was executing, since the TX isr can't be preempted.
-          // Don't enable the TX interrupt here as a means to trigger the XOFF char, because if it happens
-          // to be in the middle of trying to disable the RX interrupt in the main program, eventually the
-          // enabling of the TX interrupt could be undone. The ONLY reliable thing this can do to ensure
-          // the sending of the XOFF char is to send it HERE AND NOW.
+        // About to send the XOFF char
+        xon_xoff_state = XOFF_CHAR | XON_XOFF_CHAR_SENT;
 
-          // About to send the XOFF char
-          xon_xoff_state = XOFF_CHAR | XON_XOFF_CHAR_SENT;
+        // Wait until the TX register becomes empty and send it - Here there could be a problem
+        // - While waiting for the TX register to empty, the RX register could receive a new
+        //   character. This must also handle that situation!
+        uint32_t status;
+        while (!((status = HWUART->UART_SR) & UART_SR_TXRDY)) {
 
-          // Wait until the TX register becomes empty and send it - Here there could be a problem
-          // - While waiting for the TX register to empty, the RX register could receive a new
-          //   character. This must also handle that situation!
-          uint32_t status;
-          while (!((status = HWUART->UART_SR) & UART_SR_TXRDY)) {
+          if (status & UART_SR_RXRDY) {
+            // We received a char while waiting for the TX buffer to be empty - Receive and process it!
 
-            if (status & UART_SR_RXRDY) {
-              // We received a char while waiting for the TX buffer to be empty - Receive and process it!
+            i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
 
-              i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
+            // Read the character from the USART
+            c = HWUART->UART_RHR;
 
-              // Read the character from the USART
-              c = HWUART->UART_RHR;
+            if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c);
 
-              if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c);
-
-              // If the character is to be stored at the index just before the tail
-              // (such that the head would advance to the current tail), the FIFO is
-              // full, so don't write the character or advance the head.
-              if (i != t) {
-                rx_buffer.buffer[h] = c;
-                h = i;
-              }
-              else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
-                --rx_dropped_bytes;
+            // If the character is to be stored at the index just before the tail
+            // (such that the head would advance to the current tail), the FIFO is
+            // full, so don't write the character or advance the head.
+            if (i != t) {
+              rx_buffer.buffer[h] = c;
+              h = i;
             }
-            sw_barrier();
+            else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
+              --rx_dropped_bytes;
           }
-
-          HWUART->UART_THR = XOFF_CHAR;
-
-          // At this point there could be a race condition between the write() function
-          // and this sending of the XOFF char. This interrupt could happen between the
-          // wait to be empty TX buffer loop and the actual write of the character. Since
-          // the TX buffer is full because it's sending the XOFF char, the only way to be
-          // sure the write() function will succeed is to wait for the XOFF char to be
-          // completely sent. Since an extra character could be received during the wait
-          // it must also be handled!
-          while (!((status = HWUART->UART_SR) & UART_SR_TXRDY)) {
-
-            if (status & UART_SR_RXRDY) {
-              // A char arrived while waiting for the TX buffer to be empty - Receive and process it!
-
-              i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
-
-              // Read the character from the USART
-              c = HWUART->UART_RHR;
-
-              if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c);
-
-              // If the character is to be stored at the index just before the tail
-              // (such that the head would advance to the current tail), the FIFO is
-              // full, so don't write the character or advance the head.
-              if (i != t) {
-                rx_buffer.buffer[h] = c;
-                h = i;
-              }
-              else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
-                --rx_dropped_bytes;
-            }
-            sw_barrier();
-          }
-
-          // At this point everything is ready. The write() function won't
-          // have any issues writing to the UART TX register if it needs to!
+          sw_barrier();
         }
+
+        HWUART->UART_THR = XOFF_CHAR;
+
+        // At this point there could be a race condition between the write() function
+        // and this sending of the XOFF char. This interrupt could happen between the
+        // wait to be empty TX buffer loop and the actual write of the character. Since
+        // the TX buffer is full because it's sending the XOFF char, the only way to be
+        // sure the write() function will succeed is to wait for the XOFF char to be
+        // completely sent. Since an extra character could be received during the wait
+        // it must also be handled!
+        while (!((status = HWUART->UART_SR) & UART_SR_TXRDY)) {
+
+          if (status & UART_SR_RXRDY) {
+            // A char arrived while waiting for the TX buffer to be empty - Receive and process it!
+
+            i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
+
+            // Read the character from the USART
+            c = HWUART->UART_RHR;
+
+            if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c);
+
+            // If the character is to be stored at the index just before the tail
+            // (such that the head would advance to the current tail), the FIFO is
+            // full, so don't write the character or advance the head.
+            if (i != t) {
+              rx_buffer.buffer[h] = c;
+              h = i;
+            }
+            else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
+              --rx_dropped_bytes;
+          }
+          sw_barrier();
+        }
+
+        // At this point everything is ready. The write() function won't
+        // have any issues writing to the UART TX register if it needs to!
       }
     }
-
-    // Store the new head value
-    rx_buffer.head = h;
   }
 
-  template<typename Cfg>
-  FORCE_INLINE void MarlinSerial<Cfg>::_tx_thr_empty_irq(void) {
-    if (Cfg::TX_SIZE > 0) {
-      // Read positions
-      uint8_t t = tx_buffer.tail;
-      const uint8_t h = tx_buffer.head;
+  // Store the new head value
+  rx_buffer.head = h;
+}
 
-      if (Cfg::XONOFF) {
-        // If an XON char is pending to be sent, do it now
-        if (xon_xoff_state == XON_CHAR) {
+template<typename Cfg>
+FORCE_INLINE void MarlinSerial<Cfg>::_tx_thr_empty_irq(void) {
+  if (Cfg::TX_SIZE > 0) {
+    // Read positions
+    uint8_t t = tx_buffer.tail;
+    const uint8_t h = tx_buffer.head;
 
-          // Send the character
-          HWUART->UART_THR = XON_CHAR;
+    if (Cfg::XONOFF) {
+      // If an XON char is pending to be sent, do it now
+      if (xon_xoff_state == XON_CHAR) {
 
-          // Remember we sent it.
-          xon_xoff_state = XON_CHAR | XON_XOFF_CHAR_SENT;
+        // Send the character
+        HWUART->UART_THR = XON_CHAR;
 
-          // If nothing else to transmit, just disable TX interrupts.
-          if (h == t) HWUART->UART_IDR = UART_IDR_TXRDY;
+        // Remember we sent it.
+        xon_xoff_state = XON_CHAR | XON_XOFF_CHAR_SENT;
 
-          return;
-        }
-      }
+        // If nothing else to transmit, just disable TX interrupts.
+        if (h == t) HWUART->UART_IDR = UART_IDR_TXRDY;
 
-      // If nothing to transmit, just disable TX interrupts. This could
-      // happen as the result of the non atomicity of the disabling of RX
-      // interrupts that could end reenabling TX interrupts as a side effect.
-      if (h == t) {
-        HWUART->UART_IDR = UART_IDR_TXRDY;
         return;
       }
-
-      // There is something to TX, Send the next byte
-      const uint8_t c = tx_buffer.buffer[t];
-      t = (t + 1) & (Cfg::TX_SIZE - 1);
-      HWUART->UART_THR = c;
-      tx_buffer.tail = t;
-
-      // Disable interrupts if there is nothing to transmit following this byte
-      if (h == t) HWUART->UART_IDR = UART_IDR_TXRDY;
-    }
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::UART_ISR(void) {
-    const uint32_t status = HWUART->UART_SR;
-
-    // Data received?
-    if (status & UART_SR_RXRDY) store_rxd_char();
-
-    if (Cfg::TX_SIZE > 0) {
-      // Something to send, and TX interrupts are enabled (meaning something to send)?
-      if ((status & UART_SR_TXRDY) && (HWUART->UART_IMR & UART_IMR_TXRDY)) _tx_thr_empty_irq();
     }
 
-    // Acknowledge errors
-    if ((status & UART_SR_OVRE) || (status & UART_SR_FRAME)) {
-      if (Cfg::DROPPED_RX && (status & UART_SR_OVRE) && !++rx_dropped_bytes) --rx_dropped_bytes;
-      if (Cfg::RX_OVERRUNS && (status & UART_SR_OVRE) && !++rx_buffer_overruns) --rx_buffer_overruns;
-      if (Cfg::RX_FRAMING_ERRORS && (status & UART_SR_FRAME) && !++rx_framing_errors) --rx_framing_errors;
-
-      // TODO: error reporting outside ISR
-      HWUART->UART_CR = UART_CR_RSTSTA;
-    }
-  }
-
-  // Public Methods
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::begin(const long baud_setting) {
-
-    // Disable UART interrupt in NVIC
-    NVIC_DisableIRQ( HWUART_IRQ );
-
-    // We NEED memory barriers to ensure Interrupts are actually disabled!
-    // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
-    __DSB();
-    __ISB();
-
-    // Disable clock
-    pmc_disable_periph_clk( HWUART_IRQ_ID );
-
-    // Configure PMC
-    pmc_enable_periph_clk( HWUART_IRQ_ID );
-
-    // Disable PDC channel
-    HWUART->UART_PTCR = UART_PTCR_RXTDIS | UART_PTCR_TXTDIS;
-
-    // Reset and disable receiver and transmitter
-    HWUART->UART_CR = UART_CR_RSTRX | UART_CR_RSTTX | UART_CR_RXDIS | UART_CR_TXDIS;
-
-    // Configure mode: 8bit, No parity, 1 bit stop
-    HWUART->UART_MR = UART_MR_CHMODE_NORMAL | US_MR_CHRL_8_BIT | US_MR_NBSTOP_1_BIT | UART_MR_PAR_NO;
-
-    // Configure baudrate (asynchronous, no oversampling)
-    HWUART->UART_BRGR = (SystemCoreClock / (baud_setting << 4));
-
-    // Configure interrupts
-    HWUART->UART_IDR = 0xFFFFFFFF;
-    HWUART->UART_IER = UART_IER_RXRDY | UART_IER_OVRE | UART_IER_FRAME;
-
-    // Install interrupt handler
-    install_isr(HWUART_IRQ, UART_ISR);
-
-    // Configure priority. We need a very high priority to avoid losing characters
-    // and we need to be able to preempt the Stepper ISR and everything else!
-    // (this could probably be fixed by using DMA with the Serial port)
-    NVIC_SetPriority(HWUART_IRQ, 1);
-
-    // Enable UART interrupt in NVIC
-    NVIC_EnableIRQ(HWUART_IRQ);
-
-    // Enable receiver and transmitter
-    HWUART->UART_CR = UART_CR_RXEN | UART_CR_TXEN;
-
-    if (Cfg::TX_SIZE > 0) _written = false;
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::end() {
-    // Disable UART interrupt in NVIC
-    NVIC_DisableIRQ( HWUART_IRQ );
-
-    // We NEED memory barriers to ensure Interrupts are actually disabled!
-    // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
-    __DSB();
-    __ISB();
-
-    pmc_disable_periph_clk( HWUART_IRQ_ID );
-  }
-
-  template<typename Cfg>
-  int MarlinSerial<Cfg>::peek(void) {
-    const int v = rx_buffer.head == rx_buffer.tail ? -1 : rx_buffer.buffer[rx_buffer.tail];
-    return v;
-  }
-
-  template<typename Cfg>
-  int MarlinSerial<Cfg>::read(void) {
-
-    const ring_buffer_pos_t h = rx_buffer.head;
-    ring_buffer_pos_t t = rx_buffer.tail;
-
-    if (h == t) return -1;
-
-    int v = rx_buffer.buffer[t];
-    t = (ring_buffer_pos_t)(t + 1) & (Cfg::RX_SIZE - 1);
-
-    // Advance tail
-    rx_buffer.tail = t;
-
-    if (Cfg::XONOFF) {
-      // If the XOFF char was sent, or about to be sent...
-      if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XOFF_CHAR) {
-        // Get count of bytes in the RX buffer
-        const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
-        // When below 10% of RX buffer capacity, send XON before running out of RX buffer bytes
-        if (rx_count < (Cfg::RX_SIZE) / 10) {
-          if (Cfg::TX_SIZE > 0) {
-            // Signal we want an XON character to be sent.
-            xon_xoff_state = XON_CHAR;
-            // Enable TX isr.
-            HWUART->UART_IER = UART_IER_TXRDY;
-          }
-          else {
-            // If not using TX interrupts, we must send the XON char now
-            xon_xoff_state = XON_CHAR | XON_XOFF_CHAR_SENT;
-            while (!(HWUART->UART_SR & UART_SR_TXRDY)) sw_barrier();
-            HWUART->UART_THR = XON_CHAR;
-          }
-        }
-      }
+    // If nothing to transmit, just disable TX interrupts. This could
+    // happen as the result of the non atomicity of the disabling of RX
+    // interrupts that could end reenabling TX interrupts as a side effect.
+    if (h == t) {
+      HWUART->UART_IDR = UART_IDR_TXRDY;
+      return;
     }
 
-    return v;
+    // There is something to TX, Send the next byte
+    const uint8_t c = tx_buffer.buffer[t];
+    t = (t + 1) & (Cfg::TX_SIZE - 1);
+    HWUART->UART_THR = c;
+    tx_buffer.tail = t;
+
+    // Disable interrupts if there is nothing to transmit following this byte
+    if (h == t) HWUART->UART_IDR = UART_IDR_TXRDY;
+  }
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::UART_ISR(void) {
+  const uint32_t status = HWUART->UART_SR;
+
+  // Data received?
+  if (status & UART_SR_RXRDY) store_rxd_char();
+
+  if (Cfg::TX_SIZE > 0) {
+    // Something to send, and TX interrupts are enabled (meaning something to send)?
+    if ((status & UART_SR_TXRDY) && (HWUART->UART_IMR & UART_IMR_TXRDY)) _tx_thr_empty_irq();
   }
 
-  template<typename Cfg>
-  typename MarlinSerial<Cfg>::ring_buffer_pos_t MarlinSerial<Cfg>::available(void) {
-    const ring_buffer_pos_t h = rx_buffer.head, t = rx_buffer.tail;
-    return (ring_buffer_pos_t)(Cfg::RX_SIZE + h - t) & (Cfg::RX_SIZE - 1);
+  // Acknowledge errors
+  if ((status & UART_SR_OVRE) || (status & UART_SR_FRAME)) {
+    if (Cfg::DROPPED_RX && (status & UART_SR_OVRE) && !++rx_dropped_bytes) --rx_dropped_bytes;
+    if (Cfg::RX_OVERRUNS && (status & UART_SR_OVRE) && !++rx_buffer_overruns) --rx_buffer_overruns;
+    if (Cfg::RX_FRAMING_ERRORS && (status & UART_SR_FRAME) && !++rx_framing_errors) --rx_framing_errors;
+
+    // TODO: error reporting outside ISR
+    HWUART->UART_CR = UART_CR_RSTSTA;
   }
+}
 
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::flush(void) {
-    rx_buffer.tail = rx_buffer.head;
+// Public Methods
+template<typename Cfg>
+void MarlinSerial<Cfg>::begin(const long baud_setting) {
 
-    if (Cfg::XONOFF) {
-      if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XOFF_CHAR) {
+  // Disable UART interrupt in NVIC
+  NVIC_DisableIRQ( HWUART_IRQ );
+
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
+
+  // Disable clock
+  pmc_disable_periph_clk( HWUART_IRQ_ID );
+
+  // Configure PMC
+  pmc_enable_periph_clk( HWUART_IRQ_ID );
+
+  // Disable PDC channel
+  HWUART->UART_PTCR = UART_PTCR_RXTDIS | UART_PTCR_TXTDIS;
+
+  // Reset and disable receiver and transmitter
+  HWUART->UART_CR = UART_CR_RSTRX | UART_CR_RSTTX | UART_CR_RXDIS | UART_CR_TXDIS;
+
+  // Configure mode: 8bit, No parity, 1 bit stop
+  HWUART->UART_MR = UART_MR_CHMODE_NORMAL | US_MR_CHRL_8_BIT | US_MR_NBSTOP_1_BIT | UART_MR_PAR_NO;
+
+  // Configure baudrate (asynchronous, no oversampling)
+  HWUART->UART_BRGR = (SystemCoreClock / (baud_setting << 4));
+
+  // Configure interrupts
+  HWUART->UART_IDR = 0xFFFFFFFF;
+  HWUART->UART_IER = UART_IER_RXRDY | UART_IER_OVRE | UART_IER_FRAME;
+
+  // Install interrupt handler
+  install_isr(HWUART_IRQ, UART_ISR);
+
+  // Configure priority. We need a very high priority to avoid losing characters
+  // and we need to be able to preempt the Stepper ISR and everything else!
+  // (this could probably be fixed by using DMA with the Serial port)
+  NVIC_SetPriority(HWUART_IRQ, 1);
+
+  // Enable UART interrupt in NVIC
+  NVIC_EnableIRQ(HWUART_IRQ);
+
+  // Enable receiver and transmitter
+  HWUART->UART_CR = UART_CR_RXEN | UART_CR_TXEN;
+
+  if (Cfg::TX_SIZE > 0) _written = false;
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::end() {
+  // Disable UART interrupt in NVIC
+  NVIC_DisableIRQ( HWUART_IRQ );
+
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
+
+  pmc_disable_periph_clk( HWUART_IRQ_ID );
+}
+
+template<typename Cfg>
+int MarlinSerial<Cfg>::peek(void) {
+  const int v = rx_buffer.head == rx_buffer.tail ? -1 : rx_buffer.buffer[rx_buffer.tail];
+  return v;
+}
+
+template<typename Cfg>
+int MarlinSerial<Cfg>::read(void) {
+
+  const ring_buffer_pos_t h = rx_buffer.head;
+  ring_buffer_pos_t t = rx_buffer.tail;
+
+  if (h == t) return -1;
+
+  int v = rx_buffer.buffer[t];
+  t = (ring_buffer_pos_t)(t + 1) & (Cfg::RX_SIZE - 1);
+
+  // Advance tail
+  rx_buffer.tail = t;
+
+  if (Cfg::XONOFF) {
+    // If the XOFF char was sent, or about to be sent...
+    if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XOFF_CHAR) {
+      // Get count of bytes in the RX buffer
+      const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
+      // When below 10% of RX buffer capacity, send XON before running out of RX buffer bytes
+      if (rx_count < (Cfg::RX_SIZE) / 10) {
         if (Cfg::TX_SIZE > 0) {
           // Signal we want an XON character to be sent.
           xon_xoff_state = XON_CHAR;
@@ -384,257 +350,301 @@
     }
   }
 
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::write(const uint8_t c) {
-    _written = true;
+  return v;
+}
 
-    if (Cfg::TX_SIZE == 0) {
-      while (!(HWUART->UART_SR & UART_SR_TXRDY)) sw_barrier();
+template<typename Cfg>
+typename MarlinSerial<Cfg>::ring_buffer_pos_t MarlinSerial<Cfg>::available(void) {
+  const ring_buffer_pos_t h = rx_buffer.head, t = rx_buffer.tail;
+  return (ring_buffer_pos_t)(Cfg::RX_SIZE + h - t) & (Cfg::RX_SIZE - 1);
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::flush(void) {
+  rx_buffer.tail = rx_buffer.head;
+
+  if (Cfg::XONOFF) {
+    if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XOFF_CHAR) {
+      if (Cfg::TX_SIZE > 0) {
+        // Signal we want an XON character to be sent.
+        xon_xoff_state = XON_CHAR;
+        // Enable TX isr.
+        HWUART->UART_IER = UART_IER_TXRDY;
+      }
+      else {
+        // If not using TX interrupts, we must send the XON char now
+        xon_xoff_state = XON_CHAR | XON_XOFF_CHAR_SENT;
+        while (!(HWUART->UART_SR & UART_SR_TXRDY)) sw_barrier();
+        HWUART->UART_THR = XON_CHAR;
+      }
+    }
+  }
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::write(const uint8_t c) {
+  _written = true;
+
+  if (Cfg::TX_SIZE == 0) {
+    while (!(HWUART->UART_SR & UART_SR_TXRDY)) sw_barrier();
+    HWUART->UART_THR = c;
+  }
+  else {
+
+    // If the TX interrupts are disabled and the data register
+    // is empty, just write the byte to the data register and
+    // be done. This shortcut helps significantly improve the
+    // effective datarate at high (>500kbit/s) bitrates, where
+    // interrupt overhead becomes a slowdown.
+    // Yes, there is a race condition between the sending of the
+    // XOFF char at the RX isr, but it is properly handled there
+    if (!(HWUART->UART_IMR & UART_IMR_TXRDY) && (HWUART->UART_SR & UART_SR_TXRDY)) {
       HWUART->UART_THR = c;
+      return;
+    }
+
+    const uint8_t i = (tx_buffer.head + 1) & (Cfg::TX_SIZE - 1);
+
+    // If global interrupts are disabled (as the result of being called from an ISR)...
+    if (!ISRS_ENABLED()) {
+
+      // Make room by polling if it is possible to transmit, and do so!
+      while (i == tx_buffer.tail) {
+        // If we can transmit another byte, do it.
+        if (HWUART->UART_SR & UART_SR_TXRDY) _tx_thr_empty_irq();
+        // Make sure compiler rereads tx_buffer.tail
+        sw_barrier();
+      }
     }
     else {
-
-      // If the TX interrupts are disabled and the data register
-      // is empty, just write the byte to the data register and
-      // be done. This shortcut helps significantly improve the
-      // effective datarate at high (>500kbit/s) bitrates, where
-      // interrupt overhead becomes a slowdown.
-      // Yes, there is a race condition between the sending of the
-      // XOFF char at the RX isr, but it is properly handled there
-      if (!(HWUART->UART_IMR & UART_IMR_TXRDY) && (HWUART->UART_SR & UART_SR_TXRDY)) {
-        HWUART->UART_THR = c;
-        return;
-      }
-
-      const uint8_t i = (tx_buffer.head + 1) & (Cfg::TX_SIZE - 1);
-
-      // If global interrupts are disabled (as the result of being called from an ISR)...
-      if (!ISRS_ENABLED()) {
-
-        // Make room by polling if it is possible to transmit, and do so!
-        while (i == tx_buffer.tail) {
-          // If we can transmit another byte, do it.
-          if (HWUART->UART_SR & UART_SR_TXRDY) _tx_thr_empty_irq();
-          // Make sure compiler rereads tx_buffer.tail
-          sw_barrier();
-        }
-      }
-      else {
-        // Interrupts are enabled, just wait until there is space
-        while (i == tx_buffer.tail) sw_barrier();
-      }
-
-      // Store new char. head is always safe to move
-      tx_buffer.buffer[tx_buffer.head] = c;
-      tx_buffer.head = i;
-
-      // Enable TX isr - Non atomic, but it will eventually enable TX isr
-      HWUART->UART_IER = UART_IER_TXRDY;
+      // Interrupts are enabled, just wait until there is space
+      while (i == tx_buffer.tail) sw_barrier();
     }
+
+    // Store new char. head is always safe to move
+    tx_buffer.buffer[tx_buffer.head] = c;
+    tx_buffer.head = i;
+
+    // Enable TX isr - Non atomic, but it will eventually enable TX isr
+    HWUART->UART_IER = UART_IER_TXRDY;
   }
+}
 
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::flushTX(void) {
-    // TX
+template<typename Cfg>
+void MarlinSerial<Cfg>::flushTX(void) {
+  // TX
 
-    if (Cfg::TX_SIZE == 0) {
-      // No bytes written, no need to flush. This special case is needed since there's
-      // no way to force the TXC (transmit complete) bit to 1 during initialization.
-      if (!_written) return;
+  if (Cfg::TX_SIZE == 0) {
+    // No bytes written, no need to flush. This special case is needed since there's
+    // no way to force the TXC (transmit complete) bit to 1 during initialization.
+    if (!_written) return;
 
+    // Wait until everything was transmitted
+    while (!(HWUART->UART_SR & UART_SR_TXEMPTY)) sw_barrier();
+
+    // At this point nothing is queued anymore (DRIE is disabled) and
+    // the hardware finished transmission (TXC is set).
+
+  }
+  else {
+    // If we have never written a byte, no need to flush. This special
+    // case is needed since there is no way to force the TXC (transmit
+    // complete) bit to 1 during initialization
+    if (!_written) return;
+
+    // If global interrupts are disabled (as the result of being called from an ISR)...
+    if (!ISRS_ENABLED()) {
+
+      // Wait until everything was transmitted - We must do polling, as interrupts are disabled
+      while (tx_buffer.head != tx_buffer.tail || !(HWUART->UART_SR & UART_SR_TXEMPTY)) {
+        // If there is more space, send an extra character
+        if (HWUART->UART_SR & UART_SR_TXRDY) _tx_thr_empty_irq();
+        sw_barrier();
+      }
+
+    }
+    else {
       // Wait until everything was transmitted
-      while (!(HWUART->UART_SR & UART_SR_TXEMPTY)) sw_barrier();
-
-      // At this point nothing is queued anymore (DRIE is disabled) and
-      // the hardware finished transmission (TXC is set).
-
-    }
-    else {
-      // If we have never written a byte, no need to flush. This special
-      // case is needed since there is no way to force the TXC (transmit
-      // complete) bit to 1 during initialization
-      if (!_written) return;
-
-      // If global interrupts are disabled (as the result of being called from an ISR)...
-      if (!ISRS_ENABLED()) {
-
-        // Wait until everything was transmitted - We must do polling, as interrupts are disabled
-        while (tx_buffer.head != tx_buffer.tail || !(HWUART->UART_SR & UART_SR_TXEMPTY)) {
-          // If there is more space, send an extra character
-          if (HWUART->UART_SR & UART_SR_TXRDY) _tx_thr_empty_irq();
-          sw_barrier();
-        }
-
-      }
-      else {
-        // Wait until everything was transmitted
-        while (tx_buffer.head != tx_buffer.tail || !(HWUART->UART_SR & UART_SR_TXEMPTY)) sw_barrier();
-      }
-
-      // At this point nothing is queued anymore (DRIE is disabled) and
-      // the hardware finished transmission (TXC is set).
-    }
-  }
-
-  /**
-   * Imports from print.h
-   */
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::print(char c, int base) {
-    print((long)c, base);
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::print(unsigned char b, int base) {
-    print((unsigned long)b, base);
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::print(int n, int base) {
-    print((long)n, base);
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::print(unsigned int n, int base) {
-    print((unsigned long)n, base);
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::print(long n, int base) {
-    if (base == 0) write(n);
-    else if (base == 10) {
-      if (n < 0) { print('-'); n = -n; }
-      printNumber(n, 10);
-    }
-    else
-      printNumber(n, base);
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::print(unsigned long n, int base) {
-    if (base == 0) write(n);
-    else printNumber(n, base);
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::print(double n, int digits) {
-    printFloat(n, digits);
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::println(void) {
-    print('\r');
-    print('\n');
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::println(const String& s) {
-    print(s);
-    println();
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::println(const char c[]) {
-    print(c);
-    println();
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::println(char c, int base) {
-    print(c, base);
-    println();
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::println(unsigned char b, int base) {
-    print(b, base);
-    println();
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::println(int n, int base) {
-    print(n, base);
-    println();
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::println(unsigned int n, int base) {
-    print(n, base);
-    println();
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::println(long n, int base) {
-    print(n, base);
-    println();
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::println(unsigned long n, int base) {
-    print(n, base);
-    println();
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::println(double n, int digits) {
-    print(n, digits);
-    println();
-  }
-
-  // Private Methods
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::printNumber(unsigned long n, uint8_t base) {
-    if (n) {
-      unsigned char buf[8 * sizeof(long)]; // Enough space for base 2
-      int8_t i = 0;
-      while (n) {
-        buf[i++] = n % base;
-        n /= base;
-      }
-      while (i--)
-        print((char)(buf[i] + (buf[i] < 10 ? '0' : 'A' - 10)));
-    }
-    else
-      print('0');
-  }
-
-  template<typename Cfg>
-  void MarlinSerial<Cfg>::printFloat(double number, uint8_t digits) {
-    // Handle negative numbers
-    if (number < 0.0) {
-      print('-');
-      number = -number;
+      while (tx_buffer.head != tx_buffer.tail || !(HWUART->UART_SR & UART_SR_TXEMPTY)) sw_barrier();
     }
 
-    // Round correctly so that print(1.999, 2) prints as "2.00"
-    double rounding = 0.5;
-    for (uint8_t i = 0; i < digits; ++i) rounding *= 0.1;
-    number += rounding;
+    // At this point nothing is queued anymore (DRIE is disabled) and
+    // the hardware finished transmission (TXC is set).
+  }
+}
 
-    // Extract the integer part of the number and print it
-    unsigned long int_part = (unsigned long)number;
-    double remainder = number - (double)int_part;
-    print(int_part);
+/**
+ * Imports from print.h
+ */
 
-    // Print the decimal point, but only if there are digits beyond
-    if (digits) {
-      print('.');
-      // Extract digits from the remainder one at a time
-      while (digits--) {
-        remainder *= 10.0;
-        int toPrint = int(remainder);
-        print(toPrint);
-        remainder -= toPrint;
-      }
+template<typename Cfg>
+void MarlinSerial<Cfg>::print(char c, int base) {
+  print((long)c, base);
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::print(unsigned char b, int base) {
+  print((unsigned long)b, base);
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::print(int n, int base) {
+  print((long)n, base);
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::print(unsigned int n, int base) {
+  print((unsigned long)n, base);
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::print(long n, int base) {
+  if (base == 0) write(n);
+  else if (base == 10) {
+    if (n < 0) { print('-'); n = -n; }
+    printNumber(n, 10);
+  }
+  else
+    printNumber(n, base);
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::print(unsigned long n, int base) {
+  if (base == 0) write(n);
+  else printNumber(n, base);
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::print(double n, int digits) {
+  printFloat(n, digits);
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::println(void) {
+  print('\r');
+  print('\n');
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::println(const String& s) {
+  print(s);
+  println();
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::println(const char c[]) {
+  print(c);
+  println();
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::println(char c, int base) {
+  print(c, base);
+  println();
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::println(unsigned char b, int base) {
+  print(b, base);
+  println();
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::println(int n, int base) {
+  print(n, base);
+  println();
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::println(unsigned int n, int base) {
+  print(n, base);
+  println();
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::println(long n, int base) {
+  print(n, base);
+  println();
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::println(unsigned long n, int base) {
+  print(n, base);
+  println();
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::println(double n, int digits) {
+  print(n, digits);
+  println();
+}
+
+// Private Methods
+template<typename Cfg>
+void MarlinSerial<Cfg>::printNumber(unsigned long n, uint8_t base) {
+  if (n) {
+    unsigned char buf[8 * sizeof(long)]; // Enough space for base 2
+    int8_t i = 0;
+    while (n) {
+      buf[i++] = n % base;
+      n /= base;
+    }
+    while (i--)
+      print((char)(buf[i] + (buf[i] < 10 ? '0' : 'A' - 10)));
+  }
+  else
+    print('0');
+}
+
+template<typename Cfg>
+void MarlinSerial<Cfg>::printFloat(double number, uint8_t digits) {
+  // Handle negative numbers
+  if (number < 0.0) {
+    print('-');
+    number = -number;
+  }
+
+  // Round correctly so that print(1.999, 2) prints as "2.00"
+  double rounding = 0.5;
+  for (uint8_t i = 0; i < digits; ++i) rounding *= 0.1;
+  number += rounding;
+
+  // Extract the integer part of the number and print it
+  unsigned long int_part = (unsigned long)number;
+  double remainder = number - (double)int_part;
+  print(int_part);
+
+  // Print the decimal point, but only if there are digits beyond
+  if (digits) {
+    print('.');
+    // Extract digits from the remainder one at a time
+    while (digits--) {
+      remainder *= 10.0;
+      int toPrint = int(remainder);
+      print(toPrint);
+      remainder -= toPrint;
     }
   }
+}
+
+// If not using the USB port as serial port
+#if SERIAL_PORT >= 0
 
   // Preinstantiate
-  template class MarlinSerial<MarlinSerialCfg>;
+  template class MarlinSerial<MarlinSerialCfg1>;
 
   // Instantiate
-  MarlinSerial<MarlinSerialCfg> customizedSerial;
+  MarlinSerial<MarlinSerialCfg1> customizedSerial1;
+
+#endif
+
+#ifdef SERIAL_PORT_2
+
+  // Preinstantiate
+  template class MarlinSerial<MarlinSerialCfg2>;
+
+  // Instantiate
+  MarlinSerial<MarlinSerialCfg2> customizedSerial2;
 
 #endif
 
diff --git a/Marlin/src/HAL/HAL_DUE/MarlinSerial_Due.h b/Marlin/src/HAL/HAL_DUE/MarlinSerial_Due.h
index 7e20596b4c..609d4ab569 100644
--- a/Marlin/src/HAL/HAL_DUE/MarlinSerial_Due.h
+++ b/Marlin/src/HAL/HAL_DUE/MarlinSerial_Due.h
@@ -31,8 +31,6 @@
 
 #include "../shared/MarlinSerial.h"
 
-#if SERIAL_PORT >= 0
-
 #include <WString.h>
 
 #define DEC 10
@@ -161,21 +159,42 @@ private:
   static void printFloat(double, uint8_t);
 };
 
-// Serial port configuration
-struct MarlinSerialCfg {
-  static constexpr int PORT               = SERIAL_PORT;
-  static constexpr unsigned int RX_SIZE   = RX_BUFFER_SIZE;
-  static constexpr unsigned int TX_SIZE   = TX_BUFFER_SIZE;
-  static constexpr bool XONOFF            = bSERIAL_XON_XOFF;
-  static constexpr bool EMERGENCYPARSER   = bEMERGENCY_PARSER;
-  static constexpr bool DROPPED_RX        = bSERIAL_STATS_DROPPED_RX;
-  static constexpr bool RX_OVERRUNS       = bSERIAL_STATS_RX_BUFFER_OVERRUNS;
-  static constexpr bool RX_FRAMING_ERRORS = bSERIAL_STATS_RX_FRAMING_ERRORS;
-  static constexpr bool MAX_RX_QUEUED     = bSERIAL_STATS_MAX_RX_QUEUED;
-};
+#if SERIAL_PORT >= 0
 
-extern MarlinSerial<MarlinSerialCfg> customizedSerial;
+  // Serial port configuration
+  struct MarlinSerialCfg1 {
+    static constexpr int PORT               = SERIAL_PORT;
+    static constexpr unsigned int RX_SIZE   = RX_BUFFER_SIZE;
+    static constexpr unsigned int TX_SIZE   = TX_BUFFER_SIZE;
+    static constexpr bool XONOFF            = bSERIAL_XON_XOFF;
+    static constexpr bool EMERGENCYPARSER   = bEMERGENCY_PARSER;
+    static constexpr bool DROPPED_RX        = bSERIAL_STATS_DROPPED_RX;
+    static constexpr bool RX_OVERRUNS       = bSERIAL_STATS_RX_BUFFER_OVERRUNS;
+    static constexpr bool RX_FRAMING_ERRORS = bSERIAL_STATS_RX_FRAMING_ERRORS;
+    static constexpr bool MAX_RX_QUEUED     = bSERIAL_STATS_MAX_RX_QUEUED;
+  };
+
+  extern MarlinSerial<MarlinSerialCfg1> customizedSerial1;
 
 #endif // SERIAL_PORT >= 0
 
+#ifdef SERIAL_PORT_2
+
+  // Serial port configuration
+  struct MarlinSerialCfg2 {
+    static constexpr int PORT               = SERIAL_PORT_2;
+    static constexpr unsigned int RX_SIZE   = RX_BUFFER_SIZE;
+    static constexpr unsigned int TX_SIZE   = TX_BUFFER_SIZE;
+    static constexpr bool XONOFF            = bSERIAL_XON_XOFF;
+    static constexpr bool EMERGENCYPARSER   = bEMERGENCY_PARSER;
+    static constexpr bool DROPPED_RX        = bSERIAL_STATS_DROPPED_RX;
+    static constexpr bool RX_OVERRUNS       = bSERIAL_STATS_RX_BUFFER_OVERRUNS;
+    static constexpr bool RX_FRAMING_ERRORS = bSERIAL_STATS_RX_FRAMING_ERRORS;
+    static constexpr bool MAX_RX_QUEUED     = bSERIAL_STATS_MAX_RX_QUEUED;
+  };
+
+  extern MarlinSerial<MarlinSerialCfg2> customizedSerial2;
+
+#endif
+
 #endif // MARLINSERIAL_DUE_H
diff --git a/Marlin/src/HAL/HAL_DUE/Tone.cpp b/Marlin/src/HAL/HAL_DUE/Tone.cpp
index 55c03f5e58..ff522fa3ab 100644
--- a/Marlin/src/HAL/HAL_DUE/Tone.cpp
+++ b/Marlin/src/HAL/HAL_DUE/Tone.cpp
@@ -27,6 +27,7 @@
 
 #ifdef ARDUINO_ARCH_SAM
 
+#include "../../inc/MarlinConfig.h"
 #include "HAL.h"
 #include "HAL_timers_Due.h"
 
diff --git a/Marlin/src/gcode/control/M111.cpp b/Marlin/src/gcode/control/M111.cpp
index ff6bd64463..1410356789 100644
--- a/Marlin/src/gcode/control/M111.cpp
+++ b/Marlin/src/gcode/control/M111.cpp
@@ -60,19 +60,19 @@ void GcodeSuite::M111() {
     SERIAL_ECHOPGM(MSG_DEBUG_OFF);
     #if !defined(__AVR__) || !defined(USBCON)
       #if ENABLED(SERIAL_STATS_RX_BUFFER_OVERRUNS)
-        SERIAL_ECHOPAIR("\nBuffer Overruns: ", customizedSerial.buffer_overruns());
+        SERIAL_ECHOPAIR("\nBuffer Overruns: ", MYSERIAL0.buffer_overruns());
       #endif
 
       #if ENABLED(SERIAL_STATS_RX_FRAMING_ERRORS)
-        SERIAL_ECHOPAIR("\nFraming Errors: ", customizedSerial.framing_errors());
+        SERIAL_ECHOPAIR("\nFraming Errors: ", MYSERIAL0.framing_errors());
       #endif
 
       #if ENABLED(SERIAL_STATS_DROPPED_RX)
-        SERIAL_ECHOPAIR("\nDropped bytes: ", customizedSerial.dropped());
+        SERIAL_ECHOPAIR("\nDropped bytes: ", MYSERIAL0.dropped());
       #endif
 
       #if ENABLED(SERIAL_STATS_MAX_RX_QUEUED)
-        SERIAL_ECHOPAIR("\nMax RX Queue Size: ", customizedSerial.rxMaxEnqueued());
+        SERIAL_ECHOPAIR("\nMax RX Queue Size: ", MYSERIAL0.rxMaxEnqueued());
       #endif
     #endif //  !defined(__AVR__) || !defined(USBCON)
   }
diff --git a/Marlin/src/gcode/queue.cpp b/Marlin/src/gcode/queue.cpp
index 4f90f0f157..8e30248853 100644
--- a/Marlin/src/gcode/queue.cpp
+++ b/Marlin/src/gcode/queue.cpp
@@ -562,11 +562,11 @@ void advance_command_queue() {
 
         #if !defined(__AVR__) || !defined(USBCON)
           #if ENABLED(SERIAL_STATS_DROPPED_RX)
-            SERIAL_ECHOLNPAIR("Dropped bytes: ", customizedSerial.dropped());
+            SERIAL_ECHOLNPAIR("Dropped bytes: ", MYSERIAL0.dropped());
           #endif
 
           #if ENABLED(SERIAL_STATS_MAX_RX_QUEUED)
-            SERIAL_ECHOLNPAIR("Max RX Queue Size: ", customizedSerial.rxMaxEnqueued());
+            SERIAL_ECHOLNPAIR("Max RX Queue Size: ", MYSERIAL0.rxMaxEnqueued());
           #endif
         #endif //  !defined(__AVR__) || !defined(USBCON)