Fix and improve G-code queue (#21122)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>

Marlin/Configuration_adv.h

   //#define SERIAL_STATS_DROPPED_RX
 #endif
 
+// Monitor RX buffer usage
+// Dump an error to the serial port if the serial receive buffer overflows.
+// If you see these errors, increase the RX_BUFFER_SIZE value.
+// Not supported on all platforms.
+//#define RX_BUFFER_MONITOR
+
 /**
  * Emergency Command Parser
  *

Marlin/src/MarlinCore.cpp

  */
 inline void manage_inactivity(const bool ignore_stepper_queue=false) {
 
-  if (queue.length < BUFSIZE) queue.get_available_commands();
+  queue.get_available_commands();
 
   const millis_t ms = millis();
 

Marlin/src/core/bug_on.h

+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Copyright (c) 2021 X-Ryl669 [https://blog.cyril.by]
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// We need SERIAL_ECHOPAIR and macros.h
+#include "serial.h"
+
+#if ENABLED(POSTMORTEM_DEBUGGING)
+  // Useful macro for stopping the CPU on an unexpected condition
+  // This is used like SERIAL_ECHOPAIR, that is: a key-value call of the local variables you want
+  // to dump to the serial port before stopping the CPU.
+  #define BUG_ON(V...) do { SERIAL_ECHOPAIR(ONLY_FILENAME, __LINE__, ": "); SERIAL_ECHOLNPAIR(V); SERIAL_FLUSHTX(); *(char*)0 = 42; } while(0)
+#elif ENABLED(MARLIN_DEV_MODE)
+  // Don't stop the CPU here, but at least dump the bug on the serial port
+  #define BUG_ON(V...) do { SERIAL_ECHOPAIR(ONLY_FILENAME, __LINE__, ": BUG!\n"); SERIAL_ECHOLNPAIR(V); SERIAL_FLUSHTX(); } while(0)
+#else
+  // Release mode, let's ignore the bug
+  #define BUG_ON(V...) NOOP
+#endif

Marlin/src/core/language.h

 #define STR_WATCHDOG_FIRED                  "Watchdog timeout. Reset required."
 #define STR_ERR_KILLED                      "Printer halted. kill() called!"
 #define STR_ERR_STOPPED                     "Printer stopped due to errors. Fix the error and use M999 to restart. (Temperature is reset. Set it after restarting)"
+#define STR_ERR_SERIAL_MISMATCH             "Serial status mismatch"
 #define STR_BUSY_PROCESSING                 "busy: processing"
 #define STR_BUSY_PAUSED_FOR_USER            "busy: paused for user"
 #define STR_BUSY_PAUSED_FOR_INPUT           "busy: paused for input"

Marlin/src/core/macros.h

   #define CALL_IF_EXISTS(Return, That, Method, ...) \
     static_cast<Return>(Private::Call_ ## Method(That, ##__VA_ARGS__))
 
+  // Compile-time string manipulation
+  namespace CompileTimeString {
+    // Simple compile-time parser to find the position of the end of a string
+    constexpr const char* findStringEnd(const char *str) {
+      return *str ? findStringEnd(str + 1) : str;
+    }
+
+    // Check whether a string contains a slash
+    constexpr bool containsSlash(const char *str) {
+      return *str == '/' ? true : (*str ? containsSlash(str + 1) : false);
+    }
+    // Find the last position of the slash
+    constexpr const char* findLastSlashPos(const char* str) {
+      return *str == '/' ? (str + 1) : findLastSlashPos(str - 1);
+    }
+    // Compile-time evaluation of the last part of a file path
+    // Typically used to shorten the path to file in compiled strings
+    // CompileTimeString::baseName(__FILE__) returns "macros.h" and not /path/to/Marlin/src/core/macros.h
+    constexpr const char* baseName(const char* str) {
+      return containsSlash(str) ? findLastSlashPos(findStringEnd(str)) : str;
+    }
+  }
+
+  #define ONLY_FILENAME CompileTimeString::baseName(__FILE__)
+
 #else
 
   #define MIN_2(a,b)      ((a)<(b)?(a):(b))

Marlin/src/core/serial.cpp

   #endif
 #endif
 
+#if ENABLED(MEATPACK)
+  MeatpackSerial<decltype(_SERIAL_IMPL)> mpSerial(false, _SERIAL_IMPL);
+#endif
+
 void serialprintPGM(PGM_P str) {
   while (const char c = pgm_read_byte(str++)) SERIAL_CHAR(c);
 }

Marlin/src/core/serial.h

 #include "../inc/MarlinConfig.h"
 #include "serial_hook.h"
 
+#if ENABLED(MEATPACK)
+  #include "../feature/meatpack.h"
+#endif
+
 // Commonly-used strings in serial output
 extern const char NUL_STR[], SP_P_STR[], SP_T_STR[],
                   X_STR[], Y_STR[], Z_STR[], E_STR[],
     typedef MultiSerial<decltype(MYSERIAL0), TERN(HAS_ETHERNET, ConditionalSerial<decltype(MYSERIAL1)>, decltype(MYSERIAL1)), 0>      SerialOutputT;
   #endif
   extern SerialOutputT          multiSerial;
-  #define SERIAL_IMPL           multiSerial
+  #define _SERIAL_IMPL          multiSerial
 #else
   #define _PORT_REDIRECT(n,p)   NOOP
   #define _PORT_RESTORE(n)      NOOP
   #define SERIAL_ASSERT(P)      NOOP
-  #define SERIAL_IMPL           MYSERIAL0
+  #define _SERIAL_IMPL          MYSERIAL0
+#endif
+
+#if ENABLED(MEATPACK)
+  extern MeatpackSerial<decltype(_SERIAL_IMPL)> mpSerial;
+  #define SERIAL_IMPL          mpSerial
+#else
+  #define SERIAL_IMPL          _SERIAL_IMPL
 #endif
 
 #define SERIAL_OUT(WHAT, V...)  (void)SERIAL_IMPL.WHAT(V)
 #endif
 
 #define SERIAL_ECHOPGM_P(P)         (serialprintPGM(P))
-#define SERIAL_ECHOLNPGM_P(P)       (serialprintPGM(P "\n"))
+#define SERIAL_ECHOLNPGM_P(P)       do{ serialprintPGM(P); SERIAL_EOL(); }while(0)
 
 #define SERIAL_ECHOPGM(S)           (serialprintPGM(PSTR(S)))
 #define SERIAL_ECHOLNPGM(S)         (serialprintPGM(PSTR(S "\n")))

Marlin/src/feature/meatpack.cpp

   if (TEST(state, MPConfig_Bit_Active)) {                   // Is MeatPack active?
     if (!full_char_count) {                                 // No literal characters to fetch?
       uint8_t buf[2] = { 0, 0 };
-      register const uint8_t res = unpack_chars(c, buf);    // Decode the byte into one or two characters.
+      const uint8_t res = unpack_chars(c, buf);             // Decode the byte into one or two characters.
       if (res & kFirstCharIsLiteral) {                      // The 1st character couldn't be packed.
         ++full_char_count;                                  // So the next stream byte is a full character.
         if (res & kSecondCharIsLiteral) ++full_char_count;  // The 2nd character couldn't be packed. Another stream byte is a full character.
   #if ENABLED(MP_DEBUG)
     if (chars_decoded < 1024) {
       ++chars_decoded;
-      DEBUG_ECHOPGM("RB: ");
-      MYSERIAL.print((char)c);
-      DEBUG_EOL();
+      DEBUG_ECHOLNPAIR("RB: ", AS_CHAR(c));
     }
   #endif
 }
   }
 
   if (cmd_is_next) {                      // Were two command bytes received?
-    PORT_REDIRECT(serial_ind);
+    PORT_REDIRECT(SERIAL_PORTMASK(serial_ind));
     handle_command((MeatPack_Command)c);  // Then the byte is a MeatPack command
     cmd_is_next = false;
     return;
   if (char_out_count) {
     res = char_out_count;
     char_out_count = 0;
-    for (register uint8_t i = 0; i < res; ++i)
+    for (uint8_t i = 0; i < res; ++i)
       out[i] = (char)char_out_buf[i];
   }
   return res;

Marlin/src/feature/meatpack.h

 #pragma once
 
 #include <stdint.h>
+#include "../core/serial_hook.h"
 
 /**
  * Commands sent to MeatPack to control its behavior.
 };
 
 class MeatPack {
-private:
-  friend class GCodeQueue;
 
   // Utility definitions
   static const uint8_t kCommandByte         = 0b11111111,
                  char_out_count;  // Stores number of characters to be read out.
   static uint8_t char_out_buf[2]; // Output buffer for caching up to 2 characters
 
+public:
   // Pass in a character rx'd by SD card or serial. Automatically parses command/ctrl sequences,
   // and will control state internally.
   static void handle_rx_char(const uint8_t c, const serial_index_t serial_ind);
 
   static void reset_state();
   static void report_state();
-  static uint8_t unpacked_char(register const uint8_t in);
   static uint8_t unpack_chars(const uint8_t pk, uint8_t* __restrict const chars_out);
   static void handle_command(const MeatPack_Command c);
   static void handle_output_char(const uint8_t c);
 };
 
 extern MeatPack meatpack;
+
+// Implement the MeatPack serial class so it's transparent to rest of the code
+template <typename SerialT>
+struct MeatpackSerial : public SerialBase <MeatpackSerial < SerialT >> {
+  typedef SerialBase< MeatpackSerial<SerialT> > BaseClassT;
+
+  SerialT & out;
+
+  char serialBuffer[2];
+  uint8_t charCount;
+  uint8_t readIndex;
+
+  NO_INLINE size_t write(uint8_t c) { return out.write(c); }
+  void flush()                      { out.flush();  }
+  void begin(long br)               { out.begin(br); readIndex = 0; }
+  void end()                        { out.end(); }
+
+  void msgDone()                    { out.msgDone(); }
+  // Existing instances implement Arduino's operator bool, so use that if it's available
+  bool connected()                  { return Private::HasMember_connected<SerialT>::value ? CALL_IF_EXISTS(bool, &out, connected) : (bool)out; }
+  void flushTX()                    { CALL_IF_EXISTS(void, &out, flushTX); }
+
+  int available(uint8_t index) {
+    // There is a potential issue here with multiserial, since it'll return its decoded buffer whatever the serial index here.
+    // So, instead of doing MeatpackSerial<MultiSerial<...>> we should do MultiSerial<MeatpackSerial<...>, MeatpackSerial<...>>
+    // TODO, let's fix this later on
+
+    if (charCount) return charCount;          // The buffer still has data
+    if (out.available(index) <= 0) return 0;  // No data to read
+
+    // Don't read in read method, instead do it here, so we can make progress in the read method
+    const int r = out.read(index);
+    if (r == -1) return 0;  // This is an error from the underlying serial code
+    meatpack.handle_rx_char((uint8_t)r, index);
+    charCount = meatpack.get_result_char(serialBuffer);
+    readIndex = 0;
+
+    return charCount;
+  }
+
+  int readImpl(const uint8_t index) {
+    // Not enough char to make progress?
+    if (charCount == 0 && available(index) == 0) return -1;
+
+    charCount--;
+    return serialBuffer[readIndex++];
+  }
+
+  int read(uint8_t index) { return readImpl(index); }
+  int available()         { return available(0); }
+  int read()              { return readImpl(0); }
+
+  MeatpackSerial(const bool e, SerialT & out) : BaseClassT(e), out(out) {}
+};

Marlin/src/gcode/calibrate/M100.cpp

  * Also, there are two support functions that can be called from a developer's C code.
  *
  *    uint16_t check_for_free_memory_corruption(PGM_P const free_memory_start);
- *    void M100_dump_routine(PGM_P const title, const char * const start, const char * const end);
+ *    void M100_dump_routine(PGM_P const title, const char * const start, const uintptr_t size);
  *
  * Initial version by Roxy-3D
  */
    *  the block. If so, it may indicate memory corruption due to a bad pointer.
    *  Unexpected bytes are flagged in the right column.
    */
-  inline void dump_free_memory(char *start_free_memory, char *end_free_memory) {
+  void dump_free_memory(char *start_free_memory, char *end_free_memory) {
     //
     // Start and end the dump on a nice 16 byte boundary
     // (even though the values are not 16-byte aligned).
     }
   }
 
-  void M100_dump_routine(PGM_P const title, const char * const start, const char * const end) {
-    serialprintPGM(title);
-    SERIAL_EOL();
+  void M100_dump_routine(PGM_P const title, const char * const start, const uintptr_t size) {
+    SERIAL_ECHOLNPGM_P(title);
     //
     // Round the start and end locations to produce full lines of output
     //
+    const char * const end = start + size - 1;
     dump_free_memory(
       (char*)(uintptr_t(uint32_t(start) & ~0xFUL)), // Align to 16-byte boundary
       (char*)(uintptr_t(uint32_t(end)   |  0xFUL))  // Align end_free_memory to the 15th byte (at or above end_free_memory)
 #endif // M100_FREE_MEMORY_DUMPER
 
 inline int check_for_free_memory_corruption(PGM_P const title) {
-  serialprintPGM(title);
+  SERIAL_ECHOPGM_P(title);
 
   char *start_free_memory = free_memory_start, *end_free_memory = free_memory_end;
   int n = end_free_memory - start_free_memory;
 
-  SERIAL_ECHOPAIR("\nfmc() n=", n);
-  SERIAL_ECHOPAIR("\nfree_memory_start=", hex_address(free_memory_start));
-  SERIAL_ECHOLNPAIR("  end_free_memory=", hex_address(end_free_memory));
+  SERIAL_ECHOLNPAIR("\nfmc() n=", n,
+                    "\nfree_memory_start=", hex_address(free_memory_start),
+                    "  end=", hex_address(end_free_memory));
 
   if (end_free_memory < start_free_memory)  {
     SERIAL_ECHOPGM(" end_free_memory < Heap ");
-    // SET_INPUT_PULLUP(63);           // if the developer has a switch wired up to their controller board
-    // safe_delay(5);                  // this code can be enabled to pause the display as soon as the
-    // while ( READ(63))               // malfunction is detected.   It is currently defaulting to a switch
-    //   idle();                       // being on pin-63 which is unassigend and available on most controller
-    // safe_delay(20);                 // boards.
-    // while ( !READ(63))
-    //   idle();
+    //SET_INPUT_PULLUP(63);           // if the developer has a switch wired up to their controller board
+    //safe_delay(5);                  // this code can be enabled to pause the display as soon as the
+    //while ( READ(63))               // malfunction is detected.   It is currently defaulting to a switch
+    //  idle();                       // being on pin-63 which is unassigend and available on most controller
+    //safe_delay(20);                 // boards.
+    //while ( !READ(63))
+    //  idle();
     serial_delay(20);
     #if ENABLED(M100_FREE_MEMORY_DUMPER)
-      M100_dump_routine(PSTR("   Memory corruption detected with end_free_memory<Heap\n"), (const char*)0x1B80, (const char*)0x21FF);
+      M100_dump_routine(PSTR("   Memory corruption detected with end_free_memory<Heap\n"), (const char*)0x1B80, 0x0680);
     #endif
   }
 
     if (start_free_memory[i] == TEST_BYTE) {
       int32_t j = count_test_bytes(start_free_memory + i);
       if (j > 8) {
-        // SERIAL_ECHOPAIR("Found ", j);
-        // SERIAL_ECHOLNPAIR(" bytes free at ", hex_address(start_free_memory + i));
+        //SERIAL_ECHOPAIR("Found ", j);
+        //SERIAL_ECHOLNPAIR(" bytes free at ", hex_address(start_free_memory + i));
         i += j;
         block_cnt++;
-        SERIAL_ECHOPAIR(" (", block_cnt);
-        SERIAL_ECHOPAIR(") found=", j);
-        SERIAL_ECHOLNPGM("   ");
+        SERIAL_ECHOLNPAIR(" (", block_cnt, ") found=", j);
       }
     }
   }
     if (*addr == TEST_BYTE) {
       const int32_t j = count_test_bytes(addr);
       if (j > 8) {
-        SERIAL_ECHOPAIR("Found ", j);
-        SERIAL_ECHOLNPAIR(" bytes free at ", hex_address(addr));
+        SERIAL_ECHOLNPAIR("Found ", j, " bytes free at ", hex_address(addr));
         if (j > max_cnt) {
           max_cnt  = j;
           max_addr = addr;
       }
     }
   }
-  if (block_cnt > 1) {
-    SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area.");
-    SERIAL_ECHOPAIR("\nLargest free block is ", max_cnt);
-    SERIAL_ECHOLNPAIR(" bytes at ", hex_address(max_addr));
-  }
+  if (block_cnt > 1) SERIAL_ECHOLNPAIR(
+    "\nMemory Corruption detected in free memory area."
+    "\nLargest free block is ", max_cnt, " bytes at ", hex_address(max_addr)
+  );
   SERIAL_ECHOLNPAIR("check_for_free_memory_corruption() = ", check_for_free_memory_corruption(PSTR("M100 F ")));
 }
 
    *  Corrupt <num> locations in the free memory pool and report the corrupt addresses.
    *  This is useful to check the correctness of the M100 D and the M100 F commands.
    */
-  inline void corrupt_free_memory(char *start_free_memory, const uint32_t size) {
+  inline void corrupt_free_memory(char *start_free_memory, const uintptr_t size) {
     start_free_memory += 8;
     const uint32_t near_top = top_of_stack() - start_free_memory - 250, // -250 to avoid interrupt activity that's altered the stack.
                    j = near_top / (size + 1);
 
-    SERIAL_ECHOLNPGM("Corrupting free memory block.\n");
+    SERIAL_ECHOLNPGM("Corrupting free memory block.");
     for (uint32_t i = 1; i <= size; i++) {
       char * const addr = start_free_memory + i * j;
       *addr = i;
     return;
   }
 
-  start_free_memory += 8;       // move a few bytes away from the heap just because we don't want
-                  // to be altering memory that close to it.
+  start_free_memory += 8; // move a few bytes away from the heap just because we
+                          // don't want to be altering memory that close to it.
   memset(start_free_memory, TEST_BYTE, size);
 
   SERIAL_ECHO(size);
  * M100: Free Memory Check
  */
 void GcodeSuite::M100() {
-
   char *sp = top_of_stack();
   if (!free_memory_end) free_memory_end = sp - MEMORY_END_CORRECTION;
-  SERIAL_ECHOPAIR("\nbss_end               : ", hex_address(end_bss));
-  if (heaplimit) SERIAL_ECHOPAIR("\n__heaplimit           : ", hex_address(heaplimit));
-  SERIAL_ECHOPAIR("\nfree_memory_start     : ", hex_address(free_memory_start));
+                  SERIAL_ECHOPAIR("\nbss_end               : ", hex_address(end_bss));
+  if (heaplimit)  SERIAL_ECHOPAIR("\n__heaplimit           : ", hex_address(heaplimit));
+                  SERIAL_ECHOPAIR("\nfree_memory_start     : ", hex_address(free_memory_start));
   if (stacklimit) SERIAL_ECHOPAIR("\n__stacklimit          : ", hex_address(stacklimit));
-  SERIAL_ECHOPAIR("\nfree_memory_end       : ", hex_address(free_memory_end));
-  if (MEMORY_END_CORRECTION)  SERIAL_ECHOPAIR("\nMEMORY_END_CORRECTION: ", MEMORY_END_CORRECTION);
-  SERIAL_ECHOLNPAIR("\nStack Pointer         : ", hex_address(sp));
+                  SERIAL_ECHOPAIR("\nfree_memory_end       : ", hex_address(free_memory_end));
+  if (MEMORY_END_CORRECTION)
+                  SERIAL_ECHOPAIR("\nMEMORY_END_CORRECTION : ", MEMORY_END_CORRECTION);
+                  SERIAL_ECHOLNPAIR("\nStack Pointer       : ", hex_address(sp));
 
   // Always init on the first invocation of M100
   static bool m100_not_initialized = true;
     return free_memory_pool_report(free_memory_start, free_memory_end - free_memory_start);
 
   #if ENABLED(M100_FREE_MEMORY_CORRUPTOR)
-
     if (parser.seen('C'))
       return corrupt_free_memory(free_memory_start, parser.value_int());
-
   #endif
 }
 

Marlin/src/gcode/gcode.cpp

 
 #endif // HAS_LEVELING && G29_RETRY_AND_RECOVER
 
-//
-// Placeholders for non-migrated codes
-//
-#if ENABLED(M100_FREE_MEMORY_WATCHER)
-  extern void M100_dump_routine(PGM_P const title, const char * const start, const char * const end);
-#endif
-
 /**
  * Process the parsed command and dispatch it to its handler
  */
   SERIAL_OUT(msgDone); // Call the msgDone serial hook to signal command processing done
 }
 
+#if ENABLED(M100_FREE_MEMORY_DUMPER)
+  void M100_dump_routine(PGM_P const title, const char * const start, const uintptr_t size);
+#endif
+
 /**
  * Process a single command and dispatch it to its handler
  * This is called from the main loop()
  */
 void GcodeSuite::process_next_command() {
-  char * const current_command = queue.command_buffer[queue.index_r];
+  GCodeQueue::CommandLine &command = queue.ring_buffer.peek_next_command();
 
-  PORT_REDIRECT(SERIAL_PORTMASK(queue.port[queue.index_r]));
+  PORT_REDIRECT(SERIAL_PORTMASK(command.port));
 
-  #if ENABLED(POWER_LOSS_RECOVERY)
-    recovery.queue_index_r = queue.index_r;
-  #endif
+  TERN_(POWER_LOSS_RECOVERY, recovery.queue_index_r = queue.ring_buffer.index_r);
 
   if (DEBUGGING(ECHO)) {
     SERIAL_ECHO_START();
-    SERIAL_ECHOLN(current_command);
+    SERIAL_ECHOLN(command.buffer);
     #if ENABLED(M100_FREE_MEMORY_DUMPER)
-      SERIAL_ECHOPAIR("slot:", queue.index_r);
-      M100_dump_routine(PSTR("   Command Queue:"), &queue.command_buffer[0][0], &queue.command_buffer[BUFSIZE - 1][MAX_CMD_SIZE - 1]);
+      SERIAL_ECHOPAIR("slot:", queue.ring_buffer.index_r);
+      M100_dump_routine(PSTR("   Command Queue:"), (const char*)&queue.ring_buffer, sizeof(queue.ring_buffer));
     #endif
   }
 
   // Parse the next command in the queue
-  parser.parse(current_command);
+  parser.parse(command.buffer);
   process_parsed_command();
 }
 

Marlin/src/gcode/host/M110.cpp

 void GcodeSuite::M110() {
 
   if (parser.seenval('N'))
-    queue.last_N[queue.command_port()] = parser.value_long();
+    queue.set_current_line_number(parser.value_long());
 
 }

Marlin/src/gcode/host/M118.cpp

     while (*p == ' ') ++p;
   }
 
-  #if HAS_MULTI_SERIAL
-    PORT_REDIRECT(WITHIN(port, 0, NUM_SERIAL) ? (port ? _BV(port - 1) : SERIAL_ALL) : multiSerial.portMask);
-  #endif
+  PORT_REDIRECT(WITHIN(port, 0, NUM_SERIAL) ? (port ? SERIAL_PORTMASK(port - 1) : SERIAL_ALL) : multiSerial.portMask);
 
   if (hasE) SERIAL_ECHO_START();
   if (hasA) SERIAL_ECHOPGM("//");

Marlin/src/gcode/queue.cpp

 #include "../module/planner.h"
 #include "../module/temperature.h"
 #include "../MarlinCore.h"
+#include "../core/bug_on.h"
 
 #if ENABLED(PRINTER_EVENT_LEDS)
   #include "../feature/leds/printer_event_leds.h"
   #include "../feature/binary_stream.h"
 #endif
 
-#if ENABLED(MEATPACK)
-  #include "../feature/meatpack.h"
-#endif
-
 #if ENABLED(POWER_LOSS_RECOVERY)
   #include "../feature/powerloss.h"
 #endif
   static millis_t last_command_time = 0;
 #endif
 
-/**
- * GCode line number handling. Hosts may opt to include line numbers when
- * sending commands to Marlin, and lines will be checked for sequentiality.
- * M110 N<int> sets the current line number.
- */
-long GCodeQueue::last_N[NUM_SERIAL];
-
-/**
- * GCode Command Queue
- * A simple ring buffer of BUFSIZE command strings.
- *
- * Commands are copied into this buffer by the command injectors
- * (immediate, serial, sd card) and they are processed sequentially by
- * the main loop. The gcode.process_next_command method parses the next
- * command and hands off execution to individual handler functions.
- */
-uint8_t GCodeQueue::length = 0,  // Count of commands in the queue
-        GCodeQueue::index_r = 0, // Ring buffer read position
-        GCodeQueue::index_w = 0; // Ring buffer write position
-
-char GCodeQueue::command_buffer[BUFSIZE][MAX_CMD_SIZE];
+GCodeQueue::SerialState GCodeQueue::serial_state[NUM_SERIAL] = { 0 };
+GCodeQueue::RingBuffer GCodeQueue::ring_buffer = { 0 };
 
-/*
- * The port that the command was received on
- */
-#if HAS_MULTI_SERIAL
-  serial_index_t GCodeQueue::port[BUFSIZE];
+#if NO_TIMEOUTS > 0
+  static millis_t last_command_time = 0;
 #endif
 
 /**
  * Serial command injection
  */
 
-// Number of characters read in the current line of serial input
-static int serial_count[NUM_SERIAL] = { 0 };
-
-bool send_ok[BUFSIZE];
-
 /**
  * Next Injected PROGMEM Command pointer. (nullptr == empty)
  * Internal commands are enqueued ahead of serial / SD commands.
  */
 char GCodeQueue::injected_commands[64]; // = { 0 }
 
-GCodeQueue::GCodeQueue() {
-  // Send "ok" after commands by default
-  LOOP_L_N(i, COUNT(send_ok)) send_ok[i] = true;
-}
-
-/**
- * Check whether there are any commands yet to be executed
- */
-bool GCodeQueue::has_commands_queued() {
-  return queue.length || injected_commands_P || injected_commands[0];
-}
-
-/**
- * Clear the Marlin command queue
- */
-void GCodeQueue::clear() {
-  index_r = index_w = length = 0;
-}
 
-/**
- * Once a new command is in the ring buffer, call this to commit it
- */
-void GCodeQueue::_commit_command(bool say_ok
+void GCodeQueue::RingBuffer::commit_command(bool skip_ok
   #if HAS_MULTI_SERIAL
     , serial_index_t serial_ind/*=-1*/
   #endif
 ) {
-  send_ok[index_w] = say_ok;
-  TERN_(HAS_MULTI_SERIAL, port[index_w] = serial_ind);
+  commands[index_w].skip_ok = skip_ok;
+  TERN_(HAS_MULTI_SERIAL, commands[index_w].port = serial_ind);
   TERN_(POWER_LOSS_RECOVERY, recovery.commit_sdpos(index_w));
-  if (++index_w >= BUFSIZE) index_w = 0;
-  length++;
+  advance_pos(index_w, 1);
 }
 
 /**
  * Return true if the command was successfully added.
  * Return false for a full buffer, or if the 'command' is a comment.
  */
-bool GCodeQueue::_enqueue(const char* cmd, bool say_ok/*=false*/
+bool GCodeQueue::RingBuffer::enqueue(const char* cmd, bool skip_ok/*=true*/
   #if HAS_MULTI_SERIAL
     , serial_index_t serial_ind/*=-1*/
   #endif
 ) {
   if (*cmd == ';' || length >= BUFSIZE) return false;
-  strcpy(command_buffer[index_w], cmd);
-  _commit_command(say_ok
+  strcpy(commands[index_w].buffer, cmd);
+  commit_command(skip_ok
     #if HAS_MULTI_SERIAL
       , serial_ind
     #endif
  * Return true if the command was consumed
  */
 bool GCodeQueue::enqueue_one(const char* cmd) {
-
-  //SERIAL_ECHOPGM("enqueue_one(\"");
-  //SERIAL_ECHO(cmd);
-  //SERIAL_ECHOPGM("\") \n");
+  //SERIAL_ECHOLNPAIR("enqueue_one(\"", cmd, "\")");
 
   if (*cmd == 0 || ISEOL(*cmd)) return true;
 
-  if (_enqueue(cmd)) {
+  if (ring_buffer.enqueue(cmd)) {
     SERIAL_ECHO_MSG(STR_ENQUEUEING, cmd, "\"");
     return true;
   }
   char cmd[i + 1];
   memcpy_P(cmd, p, i);
   cmd[i] = '\0';
-  return _enqueue(cmd);
+  return ring_buffer.enqueue(cmd);
 }
 
 /**
  *   P<int>  Planner space remaining
  *   B<int>  Block queue space remaining
  */
-void GCodeQueue::ok_to_send() {
+void GCodeQueue::RingBuffer::ok_to_send() {
   #if NO_TIMEOUTS > 0
     // Start counting from the last command's execution
     last_command_time = millis();
   #endif
+  CommandLine &command = commands[index_r];
   #if HAS_MULTI_SERIAL
-    const serial_index_t serial_ind = command_port();
+    const serial_index_t serial_ind = command.port;
     if (serial_ind < 0) return;
     PORT_REDIRECT(SERIAL_PORTMASK(serial_ind));   // Reply to the serial port that sent the command
   #endif
-  if (!send_ok[index_r]) return;
+  if (command.skip_ok) return;
   SERIAL_ECHOPGM(STR_OK);
   #if ENABLED(ADVANCED_OK)
-    char* p = command_buffer[index_r];
+    char* p = command.buffer;
     if (*p == 'N') {
       SERIAL_CHAR(' ', *p++);
       while (NUMERIC_SIGNED(*p))
  * indicate that a command needs to be re-sent.
  */
 void GCodeQueue::flush_and_request_resend() {
-  const serial_index_t serial_ind = command_port();
+  const serial_index_t serial_ind = ring_buffer.command_port();
   #if HAS_MULTI_SERIAL
     if (serial_ind < 0) return;                   // Never mind. Command came from SD or Flash Drive
     PORT_REDIRECT(SERIAL_PORTMASK(serial_ind));   // Reply to the serial port that sent the command
   #endif
   SERIAL_FLUSH();
   SERIAL_ECHOPGM(STR_RESEND);
-  SERIAL_ECHOLN(last_N[serial_ind] + 1);
-  ok_to_send();
+  SERIAL_ECHOLN(serial_state[serial_ind].last_N + 1);
 }
 
-
 // Multiserial already handle the dispatch to/from multiple port by itself
 inline bool serial_data_available(uint8_t index = SERIAL_ALL) {
   if (index == SERIAL_ALL) {
     for (index = 0; index < NUM_SERIAL; index++) {
-      if (SERIAL_IMPL.available(index) > 0) return true;
+      const int a = SERIAL_IMPL.available(index);
+      #if BOTH(RX_BUFFER_MONITOR, RX_BUFFER_SIZE)
+        if (a > RX_BUFFER_SIZE - 2) {
+          PORT_REDIRECT(SERIAL_PORTMASK(index));
+          SERIAL_ERROR_MSG("RX BUF overflow, increase RX_BUFFER_SIZE: ", a);
+        }
+      #endif
+      if (a > 0) return true;
     }
     return false;
   }
-  return SERIAL_IMPL.available(index) > 0;
+  const int a = SERIAL_IMPL.available(index);
+  #if BOTH(RX_BUFFER_MONITOR, RX_BUFFER_SIZE)
+    if (a > RX_BUFFER_SIZE - 2) {
+      PORT_REDIRECT(SERIAL_PORTMASK(index));
+      SERIAL_ERROR_MSG("RX BUF overflow, increase RX_BUFFER_SIZE: ", a);
+    }
+  #endif
+
+  return a > 0;
 }
 
 inline int read_serial(const uint8_t index) { return SERIAL_IMPL.read(index); }
 void GCodeQueue::gcode_line_error(PGM_P const err, const serial_index_t serial_ind) {
   PORT_REDIRECT(SERIAL_PORTMASK(serial_ind)); // Reply to the serial port that sent the command
   SERIAL_ERROR_START();
-  serialprintPGM(err);
-  SERIAL_ECHOLN(last_N[serial_ind]);
-  while (read_serial(serial_ind) != -1);      // Clear out the RX buffer
+  SERIAL_ECHOPGM_P(err);
+  SERIAL_ECHOLN(serial_state[serial_ind].last_N);
+  while (read_serial(serial_ind) != -1) { /* nada */ } // Clear out the RX buffer. Why don't use flush here ?
   flush_and_request_resend();
-  serial_count[serial_ind] = 0;
+  serial_state[serial_ind].count = 0;
 }
 
 FORCE_INLINE bool is_M29(const char * const cmd) {  // matches "M29" & "M29 ", but not "M290", etc
  * left on the serial port.
  */
 void GCodeQueue::get_serial_commands() {
-  static char serial_line_buffer[NUM_SERIAL][MAX_CMD_SIZE];
-
-  static uint8_t serial_input_state[NUM_SERIAL] = { PS_NORMAL };
-
   #if ENABLED(BINARY_FILE_TRANSFER)
     if (card.flag.binary_mode) {
       /**
        * receive buffer (which limits the packet size to MAX_CMD_SIZE).
        * The receive buffer also limits the packet size for reliable transmission.
        */
-      binaryStream[card.transfer_port_index].receive(serial_line_buffer[card.transfer_port_index]);
+      binaryStream[card.transfer_port_index].receive(serial_state[card.transfer_port_index].line_buffer);
       return;
     }
   #endif
   // send "wait" to indicate Marlin is still waiting.
   #if NO_TIMEOUTS > 0
     const millis_t ms = millis();
-    if (length == 0 && !serial_data_available() && ELAPSED(ms, last_command_time + NO_TIMEOUTS)) {
+    if (ring_buffer.empty() && !serial_data_available() && ELAPSED(ms, last_command_time + NO_TIMEOUTS)) {
       SERIAL_ECHOLNPGM(STR_WAIT);
       last_command_time = ms;
     }
   #endif
 
-  /**
-   * Loop while serial characters are incoming and the queue is not full
-   */
-  while (length < BUFSIZE && serial_data_available()) {
+  // Loop while serial characters are incoming and the queue is not full
+  for (bool hadData = true; hadData;) {
+    // Unless a serial port has data, this will exit on next iteration
+    hadData = false;
+
     LOOP_L_N(p, NUM_SERIAL) {
+      // Check if the queue is full and exit if it is.
+      if (ring_buffer.full()) return;
 
-      const int c = read_serial(p);
-      if (c < 0) continue;
-
-      #if ENABLED(MEATPACK)
-        meatpack.handle_rx_char(uint8_t(c), p);
-        char c_res[2] = { 0, 0 };
-        const uint8_t char_count = meatpack.get_result_char(c_res);
-      #else
-        constexpr uint8_t char_count = 1;
-      #endif
+      // No data for this port ? Skip it
+      if (!serial_data_available(p)) continue;
 
-      LOOP_L_N(char_index, char_count) {
-        const char serial_char = TERN(MEATPACK, c_res[char_index], c);
+      // Ok, we have some data to process, let's make progress here
+      hadData = true;
 
-        if (ISEOL(serial_char)) {
+      const int c = read_serial(p);
+      if (c < 0) {
+        // This should never happen, let's log it
+        PORT_REDIRECT(SERIAL_PORTMASK(p));     // Reply to the serial port that sent the command
+        // Crash here to get more information why it failed
+        BUG_ON("SP available but read -1");
+        SERIAL_ERROR_MSG(STR_ERR_SERIAL_MISMATCH);
+        SERIAL_FLUSH();
+        continue;
+      }
 
-          // Reset our state, continue if the line was empty
-          if (process_line_done(serial_input_state[p], serial_line_buffer[p], serial_count[p]))
-            continue;
+      const char serial_char = (char)c;
+      SerialState &serial = serial_state[p];
 
-          char* command = serial_line_buffer[p];
+      if (ISEOL(serial_char)) {
 
-          while (*command == ' ') command++;                   // Skip leading spaces
-          char *npos = (*command == 'N') ? command : nullptr;  // Require the N parameter to start the line
+        // Reset our state, continue if the line was empty
+        if (process_line_done(serial.input_state, serial.line_buffer, serial.count))
+          continue;
 
-          if (npos) {
+        char* command = serial.line_buffer;
 
-            const bool M110 = !!strstr_P(command, PSTR("M110"));
+        while (*command == ' ') command++;                   // Skip leading spaces
+        char *npos = (*command == 'N') ? command : nullptr;  // Require the N parameter to start the line
 
-            if (M110) {
-              char* n2pos = strchr(command + 4, 'N');
-              if (n2pos) npos = n2pos;
-            }
+        if (npos) {
 
-            const long gcode_N = strtol(npos + 1, nullptr, 10);
+          const bool M110 = !!strstr_P(command, PSTR("M110"));
 
-            if (gcode_N != last_N[p] + 1 && !M110)
-              return gcode_line_error(PSTR(STR_ERR_LINE_NO), p);
+          if (M110) {
+            char* n2pos = strchr(command + 4, 'N');
+            if (n2pos) npos = n2pos;
+          }
 
-            char *apos = strrchr(command, '*');
-            if (apos) {
-              uint8_t checksum = 0, count = uint8_t(apos - command);
-              while (count) checksum ^= command[--count];
-              if (strtol(apos + 1, nullptr, 10) != checksum)
-                return gcode_line_error(PSTR(STR_ERR_CHECKSUM_MISMATCH), p);
-            }
-            else
-              return gcode_line_error(PSTR(STR_ERR_NO_CHECKSUM), p);
+          const long gcode_N = strtol(npos + 1, nullptr, 10);
 
-            last_N[p] = gcode_N;
+          if (gcode_N != serial.last_N + 1 && !M110) {
+            // In case of error on a serial port, don't prevent other serial port from making progress
+            gcode_line_error(PSTR(STR_ERR_LINE_NO), p);
+            break;
           }
-          #if ENABLED(SDSUPPORT)
-            // Pronterface "M29" and "M29 " has no line number
-            else if (card.flag.saving && !is_M29(command))
-              return gcode_line_error(PSTR(STR_ERR_NO_CHECKSUM), p);
-          #endif
 
-          //
-          // Movement commands give an alert when the machine is stopped
-          //
-
-          if (IsStopped()) {
-            char* gpos = strchr(command, 'G');
-            if (gpos) {
-              switch (strtol(gpos + 1, nullptr, 10)) {
-                case 0: case 1:
-                #if ENABLED(ARC_SUPPORT)
-                  case 2: case 3:
-                #endif
-                #if ENABLED(BEZIER_CURVE_SUPPORT)
-                  case 5:
-                #endif
-                  PORT_REDIRECT(SERIAL_PORTMASK(p));     // Reply to the serial port that sent the command
-                  SERIAL_ECHOLNPGM(STR_ERR_STOPPED);
-                  LCD_MESSAGEPGM(MSG_STOPPED);
-                  break;
-              }
+          char *apos = strrchr(command, '*');
+          if (apos) {
+            uint8_t checksum = 0, count = uint8_t(apos - command);
+            while (count) checksum ^= command[--count];
+            if (strtol(apos + 1, nullptr, 10) != checksum) {
+              // In case of error on a serial port, don't prevent other serial port from making progress
+              gcode_line_error(PSTR(STR_ERR_CHECKSUM_MISMATCH), p);
+              break;
             }
           }
+          else {
+            // In case of error on a serial port, don't prevent other serial port from making progress
+            gcode_line_error(PSTR(STR_ERR_NO_CHECKSUM), p);
+            break;
+          }
+
+          serial.last_N = gcode_N;
+        }
+        #if ENABLED(SDSUPPORT)
+          // Pronterface "M29" and "M29 " has no line number
+          else if (card.flag.saving && !is_M29(command)) {
+            gcode_line_error(PSTR(STR_ERR_NO_CHECKSUM), p);
+            break;
+          }
+        #endif
 
-          #if DISABLED(EMERGENCY_PARSER)
-            // Process critical commands early
-            if (command[0] == 'M') switch (command[3]) {
-              case '8': if (command[2] == '0' && command[1] == '1') { wait_for_heatup = false; TERN_(HAS_LCD_MENU, wait_for_user = false); } break;
-              case '2': if (command[2] == '1' && command[1] == '1') kill(M112_KILL_STR, nullptr, true); break;
-              case '0': if (command[1] == '4' && command[2] == '1') quickstop_stepper(); break;
+        //
+        // Movement commands give an alert when the machine is stopped
+        //
+
+        if (IsStopped()) {
+          char* gpos = strchr(command, 'G');
+          if (gpos) {
+            switch (strtol(gpos + 1, nullptr, 10)) {
+              case 0: case 1:
+              #if ENABLED(ARC_SUPPORT)
+                case 2: case 3:
+              #endif
+              #if ENABLED(BEZIER_CURVE_SUPPORT)
+                case 5:
+              #endif
+                PORT_REDIRECT(SERIAL_PORTMASK(p));     // Reply to the serial port that sent the command
+                SERIAL_ECHOLNPGM(STR_ERR_STOPPED);
+                LCD_MESSAGEPGM(MSG_STOPPED);
+                break;
             }
-          #endif
+          }
+        }
 
-          #if defined(NO_TIMEOUTS) && NO_TIMEOUTS > 0
-            last_command_time = ms;
-          #endif
+        #if DISABLED(EMERGENCY_PARSER)
+          // Process critical commands early
+          if (command[0] == 'M') switch (command[3]) {
+            case '8': if (command[2] == '0' && command[1] == '1') { wait_for_heatup = false; TERN_(HAS_LCD_MENU, wait_for_user = false); } break;
+            case '2': if (command[2] == '1' && command[1] == '1') kill(M112_KILL_STR, nullptr, true); break;
+            case '0': if (command[1] == '4' && command[2] == '1') quickstop_stepper(); break;
+          }
+        #endif
 
-          // Add the command to the queue
-          _enqueue(serial_line_buffer[p], true
-            #if HAS_MULTI_SERIAL
-              , p
-            #endif
-          );
-        }
-        else
-          process_stream_char(serial_char, serial_input_state[p], serial_line_buffer[p], serial_count[p]);
+        #if NO_TIMEOUTS > 0
+          last_command_time = ms;
+        #endif
 
-      } // char_count loop
+        // Add the command to the queue
+        ring_buffer.enqueue(serial.line_buffer, false
+          #if HAS_MULTI_SERIAL
+            , p
+          #endif
+        );
+      }
+      else
+        process_stream_char(serial_char, serial.input_state, serial.line_buffer, serial.count);
 
     } // NUM_SERIAL loop
   } // queue has space, serial has data
     if (!IS_SD_PRINTING()) return;
 
     int sd_count = 0;
-    while (length < BUFSIZE && !card.eof()) {
+    while (!ring_buffer.full() && !card.eof()) {
       const int16_t n = card.get();
       const bool card_eof = card.eof();
       if (n < 0 && !card_eof) { SERIAL_ERROR_MSG(STR_SD_ERR_READ); continue; }
 
+      CommandLine &command = ring_buffer.commands[ring_buffer.index_w];
       const char sd_char = (char)n;
       const bool is_eol = ISEOL(sd_char);
       if (is_eol || card_eof) {
 
+
         // Reset stream state, terminate the buffer, and commit a non-empty command
         if (!is_eol && sd_count) ++sd_count;          // End of file with no newline
-        if (!process_line_done(sd_input_state, command_buffer[index_w], sd_count)) {
+        if (!process_line_done(sd_input_state, command.buffer, sd_count)) {
 
           // M808 L saves the sdpos of the next line. M808 loops to a new sdpos.
-          TERN_(GCODE_REPEAT_MARKERS, repeat.early_parse_M808(command_buffer[index_w]));
+          TERN_(GCODE_REPEAT_MARKERS, repeat.early_parse_M808(command.buffer));
 
           // Put the new command into the buffer (no "ok" sent)
-          _commit_command(false);
+          ring_buffer.commit_command(true);
 
-          // Prime Power-Loss Recovery for the NEXT _commit_command
+          // Prime Power-Loss Recovery for the NEXT commit_command
           TERN_(POWER_LOSS_RECOVERY, recovery.cmd_sdpos = card.getIndex());
         }
 
         if (card.eof()) card.fileHasFinished();         // Handle end of file reached
       }
       else
-        process_stream_char(sd_char, sd_input_state, command_buffer[index_w], sd_count);
+        process_stream_char(sd_char, sd_input_state, command.buffer, sd_count);
     }
   }
 
  *  - The SD card file being actively printed
  */
 void GCodeQueue::get_available_commands() {
+  if (ring_buffer.full()) return;
 
   get_serial_commands();
 
   if (process_injected_command_P() || process_injected_command()) return;
 
   // Return if the G-code buffer is empty
-  if (!length) return;
+  if (ring_buffer.empty()) return;
 
   #if ENABLED(SDSUPPORT)
 
     if (card.flag.saving) {
-      char* command = command_buffer[index_r];
-      if (is_M29(command)) {
+      char * const cmd = ring_buffer.peek_next_command_string();
+      if (is_M29(cmd)) {
         // M29 closes the file
         card.closefile();
         SERIAL_ECHOLNPGM(STR_FILE_SAVED);
       }
       else {
         // Write the string from the read buffer to SD
-        card.write_command(command);
+        card.write_command(cmd);
         if (card.flag.logging)
           gcode.process_next_command(); // The card is saving because it's logging
         else
   #endif // SDSUPPORT
 
   // The queue may be reset by a command handler or by code invoked by idle() within a handler
-  --length;
-  if (++index_r >= BUFSIZE) index_r = 0;
-
+  ring_buffer.advance_pos(ring_buffer.index_r, -1);
 }

Marlin/src/gcode/queue.h

 class GCodeQueue {
 public:
   /**
-   * GCode line number handling. Hosts may include line numbers when sending
-   * commands to Marlin, and lines will be checked for sequentiality.
-   * M110 N<int> sets the current line number.
+   * The buffers per serial port.
    */
+  struct SerialState {
+    /**
+     * GCode line number handling. Hosts may include line numbers when sending
+     * commands to Marlin, and lines will be checked for sequentiality.
+     * M110 N<int> sets the current line number.
+     */
+    long last_N;
+    int count;                        //!< Number of characters read in the current line of serial input
+    char line_buffer[MAX_CMD_SIZE];   //!< The current line accumulator
+    uint8_t input_state;              //!< The input state
+  };
 
-  static long last_N[NUM_SERIAL];
+  static SerialState serial_state[NUM_SERIAL]; //!< Serial states for each serial port
 
   /**
    * GCode Command Queue
-   * A simple ring buffer of BUFSIZE command strings.
+   * A simple (circular) ring buffer of BUFSIZE command strings.
    *
    * Commands are copied into this buffer by the command injectors
    * (immediate, serial, sd card) and they are processed sequentially by
    * the main loop. The gcode.process_next_command method parses the next
    * command and hands off execution to individual handler functions.
    */
-  static uint8_t length,  // Count of commands in the queue
-                 index_r; // Ring buffer read position
-
-  static char command_buffer[BUFSIZE][MAX_CMD_SIZE];
+  struct CommandLine {
+    char buffer[MAX_CMD_SIZE];                    //!< The command buffer
+    bool skip_ok;                                 //!< Skip sending ok when command is processed?
+    TERN_(HAS_MULTI_SERIAL, serial_index_t port); //!< Serial port the command was received on
+  };
 
   /**
-   * The port that the command was received on
+   * A handy ring buffer type
    */
-  #if HAS_MULTI_SERIAL
-    static serial_index_t port[BUFSIZE];
-  #endif
-  static inline serial_index_t command_port() { return TERN0(HAS_MULTI_SERIAL, port[index_r]); }
+  struct RingBuffer {
+    uint8_t length,                 //!< Number of commands in the queue
+            index_r,                //!< Ring buffer's read position
+            index_w;                //!< Ring buffer's write position
+    CommandLine commands[BUFSIZE];  //!< The ring buffer of commands
+
+    inline serial_index_t command_port() const { return TERN0(HAS_MULTI_SERIAL, commands[index_r].port); }
+
+    inline void clear() { length = index_r = index_w = 0; }
+
+    void advance_pos(uint8_t &p, const int inc) { if (++p >= BUFSIZE) p = 0; length += inc; }
+
+    void commit_command(bool skip_ok
+      #if HAS_MULTI_SERIAL
+        , serial_index_t serial_ind=-1
+      #endif
+    );
+
+    bool enqueue(const char* cmd, bool skip_ok = true
+      #if HAS_MULTI_SERIAL
+        , serial_index_t serial_ind=-1
+      #endif
+    );
+
+    void ok_to_send();
+
+    inline bool full(uint8_t cmdCount=1) const { return length > (BUFSIZE - cmdCount); }
+
+    inline bool empty() const { return length == 0; }
 
-  GCodeQueue();
+    inline CommandLine& peek_next_command() { return commands[index_r]; }
+
+    inline char* peek_next_command_string() { return peek_next_command().buffer; }
+  };
+
+  /**
+   * The ring buffer of commands
+   */
+  static RingBuffer ring_buffer;
 
   /**
    * Clear the Marlin command queue
    */
-  static void clear();
+  static void clear() { ring_buffer.clear(); }
 
   /**
    * Next Injected Command (PROGMEM) pointer. (nullptr == empty)
   /**
    * Check whether there are any commands yet to be executed
    */
-  static bool has_commands_queued();
+  static bool has_commands_queued() { return ring_buffer.length || injected_commands_P || injected_commands[0]; }
 
   /**
    * Get the next command in the queue, optionally log it to SD, then dispatch it
    *   P<int>  Planner space remaining
    *   B<int>  Block queue space remaining
    */
-  static void ok_to_send();
+  static inline void ok_to_send() { ring_buffer.ok_to_send(); }
 
   /**
    * Clear the serial line and request a resend of
    */
   static void flush_and_request_resend();
 
-private:
+  /**
+   * (Re)Set the current line number for the last received command
+   */
+  static inline void set_current_line_number(long n) { serial_state[ring_buffer.command_port()].last_N = n; }
 
-  static uint8_t index_w;  // Ring buffer write position
+private:
 
   static void get_serial_commands();
 
     static void get_sdcard_commands();
   #endif
 
-  static void _commit_command(bool say_ok
-    #if HAS_MULTI_SERIAL
-      , serial_index_t serial_ind=-1
-    #endif
-  );
-
-  static bool _enqueue(const char* cmd, bool say_ok=false
-    #if HAS_MULTI_SERIAL
-      , serial_index_t serial_ind=-1
-    #endif
-  );
-
   // Process the next "immediate" command (PROGMEM)
   static bool process_injected_command_P();
 
 
   static void gcode_line_error(PGM_P const err, const serial_index_t serial_ind);
 
+  friend class GcodeSuite;
 };
 
 extern GCodeQueue queue;

Marlin/src/lcd/extui/lib/mks_ui/draw_keyboard.cpp

           draw_return_ui();
           break;
         case GCodeCommand:
-          if (queue.length <= (BUFSIZE - 3)) {
+          if (!queue.ring_buffer.full(3)) {
             // Hook anything that goes to the serial port
             MYSERIAL0.setHook(lv_serial_capt_hook, lv_eom_hook, 0);
             queue.enqueue_one_now(ret_ta_txt);

Marlin/src/lcd/extui/lib/mks_ui/draw_manuaLevel.cpp

 
   switch (obj->mks_obj_id) {
     case ID_M_POINT1 ... ID_M_POINT5:
-      if (queue.length == 0) {
+      if (queue.ring_buffer.empty()) {
         if (uiCfg.leveling_first_time) {
           uiCfg.leveling_first_time = false;
           queue.inject_P(G28_STR);

Marlin/src/lcd/extui/lib/mks_ui/draw_move_motor.cpp

 static void event_handler(lv_obj_t *obj, lv_event_t event) {
   char str_1[16];
   if (event != LV_EVENT_RELEASED) return;
-  if (queue.length <= (BUFSIZE - 3)) {
+  if (!queue.ring_buffer.full(3)) {
     bool do_inject = true;
     float dist = uiCfg.move_dist;
     switch (obj->mks_obj_id) {

Marlin/src/lcd/extui/lib/mks_ui/wifi_module.cpp

       if (wifiTransError.flag != 0x1) WIFI_IO1_RESET();
       getDataF = 1;
     }
-    if (need_ok_later &&  (queue.length < BUFSIZE)) {
+    if (need_ok_later && !queue.ring_buffer.full()) {
       need_ok_later = false;
       send_to_wifi((uint8_t *)"ok\r\n", strlen("ok\r\n"));
     }
   static int wifi_read_count = 0;
 
   if (espGcodeFifo.wait_tick > 5) {
-    while ((queue.length < BUFSIZE) && (espGcodeFifo.r != espGcodeFifo.w)) {
+    while (!queue.ring_buffer.full() && (espGcodeFifo.r != espGcodeFifo.w)) {
 
       espGcodeFifo.wait_tick = 0;
 

docs/Queue.md

+# Marlin's command queue concept
+
+Marlin Firmware processes G-code commands as they arrive from multiple sources, including the SD card and one or more serial ports such as USB-connected hosts, WiFi, Bluetooth, and so on.
+
+Marlin is also continuously processing the commands at the front of the queue, converting them into signals for many physical actuators such as motors, heaters, lasers, and RGB LEDs.
+
+The firmware needs to maintain continuity and timing so the command senders remain unblocked, while still performing physical movements and other actions in real-time, respecting the physical limits of stepper motors and other peripherals.
+
+To keep things flowing Marlin feeds a single queue of G-code commands from all inputs, inserting them in the order received. Movement commands immediately go into the Planner Buffer, if there is room. The buffering of a move is considered the completion of the command, so if a non-movement command has to occur after a move is done, and not just after a move is buffered, then there has to be an `M400` to wait for the Planner Buffer to finish.
+
+Whenever the command queue gets full the sender needs to wait for space to open up, and the host may need to re-send the last command again. Marlin does some handshaking to keep the host informed during a print job, described below.
+
+An opposite problem called "planner starvation" occurs when Marlin receives many short and fast moves in a row so the Planner Buffer gets completed very quickly. In this case the host can't send commands fast enough to prevent the Planner Buffer from emptying out. Planner starvation causes obvious stuttering and is commonly seen on overloaded deltabots during small curves. Marlin has strategies to mitigate this issue, but sometimes a model has to be re-sliced (or the G-code has to be post-processed with Arc Welder) just to stay within the machine's inherent limits.
+
+Here's a basic flowchart of Marlin command processing:
+```
++------+                                Marlin's GCodeQueue
+|      |                             +--------------------------------------+     +-----------+
+| Host |                             |  SerialState         RingBuffer      |     |           |
+|      |             Marlin          |  NUM_SERIAL           BUF_SIZE       |     | Marlin    |
++--+---+        R/TX_BUFFER_SIZE     |    +---+        +------------------+ |     |           |
+   |             +------------+      |    |   |        |                  | |     | GCode     |
+   |             |            |      |    |   |        |   MAX_CMD_SIZE   +-+-----> processor |
+   |             | Platform   |      |    |   | On EOL | +--------------+ | r_pos |           |
+   +-------------> serial's   +----------->   +--------> |   G-code     | | |     +-----------+
+                 | buffer     |      |    |   |  w_pos | |   command    | | |
+                 |            |      |    |   |        | |    line      | | |
+                 +------------+      |    +---+        | +--------------+ | |
+                                     |  Line buffer    |    x BUF_SIZE    | |
+                                     |                 |                  | |
+                                     |                 |                  | |
+                                     |                 |                  | |
+                                     |                 |                  | |
+                                     |                 +------------------+ |
+                                     |                                      |
+                                     |                                      |
+                                     |                                      |
+                                     +--------------------------------------+
+```
+
+Marlin is a single-threaded application with a main `loop()` that manages the command queue and an `idle()` routine that manages the hardware. The command queue is handled in two stages:
+1. The `idle()` routine reads all inputs and attempts to enqueue any completed command lines.
+2. The main `loop()` gets the command at the front the G-code queue (if any) and runs it. Each G-code command blocks the main loop, preventing the queue from advancing until it returns. To keep essential tasks and the UI running, any commands that run a long process need to call `idle()` frequently.
+
+## Synchronization
+
+To maintain synchronization Marlin replies "`ok`" to the host as soon as the command has been enqueued. This lets the host know that it can send another command, and well-behaved hosts will wait for this message. With `ADVANCED_OK` enabled the `ok` message includes extra information (such as the number of slots left in the queue).
+
+If no data is available on the serial buffer, Marlin can be configured to periodically send a "`wait`" message to the host. This was the only method of "host keepalive" provided in Marlin 1.0, but today the better options are `HOST_KEEPALIVE` and `ADVANCED_OK`.
+
+## Limitation of the design
+
+Some limitations to the design are evident:
+1. Whenever the G-code processor is busy processing a command, the G-code queue cannot advance.
+2. A long command like `G29` causes commands to pile up and to fill the queue, making the host wait.
+3. Each serial input requires a buffer large enough for a complete G-code line. This is set by `MAX_CMD_SIZE` with a default value of 96.
+4. Since serial buffer sizes are likely used as ring buffers themselves, as an optimization their sizes must be a power of 2 (64 or 128 bytes recommended).
+5. If a host sends too much G-code at once it can saturate the `GCodeQueue`. This doesn't do anything to improve the processing rate of Marlin since only one command can be dispatched per loop iteration.
+6. With the previous point in mind, it's clear that the longstanding wisdom that you don't need a large `BUF_SIZE` is not just apocryphal. The default value of 4 is typically just fine for a single serial port. (And, if you decide to send a `G25` to pause the machine, the wait will be much shorter!)