G-code queue singleton, front injection (#14236)

Marlin/src/Marlin.cpp

     #endif // HOST_ACTION_COMMANDS
 
     if (run_runout_script)
-      enqueue_and_echo_commands_front_P(PSTR(FILAMENT_RUNOUT_SCRIPT));
+      queue.inject_P(PSTR(FILAMENT_RUNOUT_SCRIPT));
   }
 
 #endif // HAS_FILAMENT_SENSOR
     runout.run();
   #endif
 
-  if (commands_in_queue < BUFSIZE) get_available_commands();
+  if (queue.length < BUFSIZE) queue.get_available_commands();
 
   const millis_t ms = millis();
 
     const int HOME_DEBOUNCE_DELAY = 2500;
     if (!IS_SD_PRINTING() && !READ(HOME_PIN)) {
       if (!homeDebounceCount) {
-        enqueue_and_echo_commands_P(PSTR("G28"));
+        queue.inject_P(PSTR("G28"));
         LCD_MESSAGEPGM(MSG_AUTO_HOME);
       }
       if (homeDebounceCount < HOME_DEBOUNCE_DELAY)
   #endif
 
   if (IsRunning()) {
-    Stopped_gcode_LastN = gcode_LastN; // Save last g_code for restart
+    queue.stop();
     SERIAL_ERROR_MSG(MSG_ERR_STOPPED);
     LCD_MESSAGEPGM(MSG_STOPPED);
     safe_delay(350);       // allow enough time for messages to get out before stopping
   SERIAL_ECHO_START();
   SERIAL_ECHOLNPAIR(MSG_FREE_MEMORY, freeMemory(), MSG_PLANNER_BUFFER_BYTES, (int)sizeof(block_t) * (BLOCK_BUFFER_SIZE));
 
-  queue_setup();
-
   // UI must be initialized before EEPROM
   // (because EEPROM code calls the UI).
   ui.init();
             true
           #endif
         );
-        clear_command_queue();
+        queue.clear();
         quickstop_stepper();
         print_job_timer.stop();
         #if DISABLED(SD_ABORT_NO_COOLDOWN)
           card.removeJobRecoveryFile();
         #endif
         #ifdef EVENT_GCODE_SD_STOP
-          enqueue_and_echo_commands_P(PSTR(EVENT_GCODE_SD_STOP));
+          queue.inject_P(PSTR(EVENT_GCODE_SD_STOP));
         #endif
       }
 
     #endif // SDSUPPORT
 
-    if (commands_in_queue < BUFSIZE) get_available_commands();
-    advance_command_queue();
+    if (queue.length < BUFSIZE) queue.get_available_commands();
+    queue.advance();
     endstops.event_handler();
     idle();
   }

Marlin/src/core/macros.h

 #define NANOSECONDS_PER_CYCLE (1000000000.0 / F_CPU)
 
 // Remove compiler warning on an unused variable
-#define UNUSED(X) (void)X
+#ifndef UNUSED
+  #define UNUSED(x) ((void)(x))
+#endif
 
 // Macros to make a string from a macro
 #define STRINGIFY_(M) #M

Marlin/src/feature/host_actions.cpp

         break;
       case PROMPT_PAUSE_RESUME:
         msg = PSTR("LCD_PAUSE_RESUME");
-        enqueue_and_echo_commands_P(PSTR("M24"));
+        queue.inject_P(PSTR("M24"));
         break;
       case PROMPT_INFO:
         msg = PSTR("GCODE_INFO");

Marlin/src/feature/power_loss_recovery.cpp

     if (card.isDetected()) {
       load();
       if (!valid()) return purge();
-      enqueue_and_echo_commands_P(PSTR("M1000 S"));
+      queue.inject_P(PSTR("M1000 S"));
     }
   }
 }
     info.relative_modes_e = gcode.axis_relative_modes[E_AXIS];
 
     // Commands in the queue
-    info.commands_in_queue = save_queue ? commands_in_queue : 0;
-    info.cmd_queue_index_r = cmd_queue_index_r;
-    COPY(info.command_queue, command_queue);
+    info.queue_length = save_queue ? queue.length : 0;
+    info.queue_index_r = queue.index_r;
+    COPY(info.queue_buffer, queue.buffer);
 
     // Elapsed print job time
     info.print_job_elapsed = print_job_timer.duration();
   #endif
 
   // Process commands from the old pending queue
-  uint8_t c = info.commands_in_queue, r = info.cmd_queue_index_r;
+  uint8_t c = info.queue_length, r = info.queue_index_r;
   for (; c--; r = (r + 1) % BUFSIZE)
-    gcode.process_subcommands_now(info.command_queue[r]);
+    gcode.process_subcommands_now(info.queue_buffer[r]);
 
   // Resume the SD file from the last position
   char *fn = info.sd_filename;
           DEBUG_EOL();
           DEBUG_ECHOLNPAIR("retract_hop: ", info.retract_hop);
         #endif
-        DEBUG_ECHOLNPAIR("cmd_queue_index_r: ", int(info.cmd_queue_index_r));
-        DEBUG_ECHOLNPAIR("commands_in_queue: ", int(info.commands_in_queue));
-        for (uint8_t i = 0; i < info.commands_in_queue; i++) DEBUG_ECHOLNPAIR("> ", info.command_queue[i]);
+        DEBUG_ECHOLNPAIR("queue_index_r: ", int(info.queue_index_r));
+        DEBUG_ECHOLNPAIR("queue_length: ", int(info.queue_length));
+        for (uint8_t i = 0; i < info.queue_length; i++) DEBUG_ECHOLNPAIR("> ", info.queue_buffer[i]);
         DEBUG_ECHOLNPAIR("sd_filename: ", info.sd_filename);
         DEBUG_ECHOLNPAIR("sdpos: ", info.sdpos);
         DEBUG_ECHOLNPAIR("print_job_elapsed: ", info.print_job_elapsed);

Marlin/src/feature/power_loss_recovery.h

   bool relative_mode, relative_modes_e;
 
   // Command queue
-  uint8_t commands_in_queue, cmd_queue_index_r;
-  char command_queue[BUFSIZE][MAX_CMD_SIZE];
+  uint8_t queue_length, queue_index_r;
+  char queue_buffer[BUFSIZE][MAX_CMD_SIZE];
 
   // SD Filename and position
   char sd_filename[MAXPATHNAMELENGTH];

Marlin/src/feature/prusa_MMU2/mmu2.cpp

 }
 
 void MMU2::filament_runout() {
-  enqueue_and_echo_commands_P(PSTR(MMU2_FILAMENT_RUNOUT_SCRIPT));
+  queue.inject_P(PSTR(MMU2_FILAMENT_RUNOUT_SCRIPT));
   planner.synchronize();
 }
 

Marlin/src/gcode/bedlevel/abl/G29.cpp

     #ifdef Z_PROBE_END_SCRIPT
       if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT);
       planner.synchronize();
-      enqueue_and_echo_commands_P(PSTR(Z_PROBE_END_SCRIPT));
+      queue.inject_P(PSTR(Z_PROBE_END_SCRIPT));
     #endif
 
     // Auto Bed Leveling is complete! Enable if possible.

Marlin/src/gcode/bedlevel/mbl/G29.cpp

       mbl.reset();
       mbl_probe_index = 0;
       if (!ui.wait_for_bl_move) {
-        enqueue_and_echo_commands_P(PSTR("G28\nG29 S2"));
+        queue.inject_P(PSTR("G28\nG29 S2"));
         return;
       }
       state = MeshNext;

Marlin/src/gcode/calibrate/M100.cpp

       SERIAL_CHAR('|');                   // Point out non test bytes
       for (uint8_t i = 0; i < 16; i++) {
         char ccc = (char)start_free_memory[i]; // cast to char before automatically casting to char on assignment, in case the compiler is broken
-        if (&start_free_memory[i] >= (char*)command_queue && &start_free_memory[i] < (char*)command_queue + sizeof(command_queue)) { // Print out ASCII in the command buffer area
+        if (&start_free_memory[i] >= (char*)queue.buffer && &start_free_memory[i] < (char*)queue.buffer + sizeof(queue.buffer)) { // Print out ASCII in the command buffer area
           if (!WITHIN(ccc, ' ', 0x7E)) ccc = ' ';
         }
         else { // If not in the command buffer area, flag bytes that don't match the test byte

Marlin/src/gcode/control/M999.cpp

 
   if (parser.boolval('S')) return;
 
-  // gcode_LastN = Stopped_gcode_LastN;
-  flush_and_request_resend();
+  queue.flush_and_request_resend();
 }

Marlin/src/gcode/gcode.cpp

 /**
  * Process the parsed command and dispatch it to its handler
  */
-void GcodeSuite::process_parsed_command(const bool no_ok) {
+void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
   KEEPALIVE_STATE(IN_HANDLER);
 
   // Handle a known G, M, or T
 
   KEEPALIVE_STATE(NOT_BUSY);
 
-  if (!no_ok) ok_to_send();
+  if (!no_ok) queue.ok_to_send();
 }
 
 /**
  * This is called from the main loop()
  */
 void GcodeSuite::process_next_command() {
-  char * const current_command = command_queue[cmd_queue_index_r];
+  char * const current_command = queue.buffer[queue.index_r];
 
-  PORT_REDIRECT(command_queue_port[cmd_queue_index_r]);
+  PORT_REDIRECT(queue.port[queue.index_r]);
 
   if (DEBUGGING(ECHO)) {
     SERIAL_ECHO_START();
     SERIAL_ECHOLN(current_command);
     #if ENABLED(M100_FREE_MEMORY_DUMPER)
-      SERIAL_ECHOPAIR("slot:", cmd_queue_index_r);
-      M100_dump_routine(PSTR("   Command Queue:"), (const char*)command_queue, (const char*)(command_queue) + sizeof(command_queue));
+      SERIAL_ECHOPAIR("slot:", queue.index_r);
+      M100_dump_routine(PSTR("   Command Queue:"), queue.buffer, queue.buffer + sizeof(queue.buffer));
     #endif
   }
 

Marlin/src/gcode/gcode.h

   static void process_parsed_command(const bool no_ok=false);
   static void process_next_command();
 
-  // Execute G-code as a macro, preserving parser state
+  // Execute G-code in-place, preserving current G-code parameters
   static void process_subcommands_now_P(PGM_P pgcode);
   static void process_subcommands_now(char * gcode);
 

Marlin/src/gcode/host/M110.cpp

  */
 
 #include "../gcode.h"
-#include "../queue.h" // for gcode_LastN
+#include "../queue.h" // for last_N
 
 /**
  * M110: Set Current Line Number
  */
 void GcodeSuite::M110() {
-  if (parser.seenval('N')) gcode_LastN = parser.value_long();
+  if (parser.seenval('N')) queue.last_N = parser.value_long();
 }

Marlin/src/gcode/queue.cpp

  */
 
 #include "queue.h"
+GCodeQueue queue;
+
 #include "gcode.h"
 
 #include "../lcd/ultralcd.h"
  * sending commands to Marlin, and lines will be checked for sequentiality.
  * M110 N<int> sets the current line number.
  */
-long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;
+long gcode_N, GCodeQueue::last_N, GCodeQueue::stopped_N = 0;
 
 /**
  * GCode Command Queue
  * the main loop. The gcode.process_next_command method parses the next
  * command and hands off execution to individual handler functions.
  */
-uint8_t commands_in_queue = 0, // Count of commands in the queue
-        cmd_queue_index_r = 0, // Ring buffer read position
-        cmd_queue_index_w = 0; // Ring buffer write position
+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 command_queue[BUFSIZE][MAX_CMD_SIZE];
+char GCodeQueue::buffer[BUFSIZE][MAX_CMD_SIZE];
 
 /*
  * The port that the command was received on
  */
 #if NUM_SERIAL > 1
-  int16_t command_queue_port[BUFSIZE];
+  int16_t GCodeQueue::port[BUFSIZE];
 #endif
 
 /**
  */
 static PGM_P injected_commands_P = nullptr;
 
-void queue_setup() {
+GCodeQueue::GCodeQueue() {
   // Send "ok" after commands by default
   for (uint8_t i = 0; i < COUNT(send_ok); i++) send_ok[i] = true;
 }
 /**
  * Clear the Marlin command queue
  */
-void clear_command_queue() {
-  cmd_queue_index_r = cmd_queue_index_w = commands_in_queue = 0;
+void GCodeQueue::clear() {
+  index_r = index_w = length = 0;
 }
 
 /**
  * Once a new command is in the ring buffer, call this to commit it
  */
-inline void _commit_command(bool say_ok
+void GCodeQueue::_commit_command(bool say_ok
   #if NUM_SERIAL > 1
-    , int16_t port = -1
+    , int16_t p/*=-1*/
   #endif
 ) {
-  send_ok[cmd_queue_index_w] = say_ok;
+  send_ok[index_w] = say_ok;
   #if NUM_SERIAL > 1
-    command_queue_port[cmd_queue_index_w] = port;
+    port[index_w] = p;
   #endif
-  if (++cmd_queue_index_w >= BUFSIZE) cmd_queue_index_w = 0;
-  commands_in_queue++;
+  if (++index_w >= BUFSIZE) index_w = 0;
+  length++;
 }
 
 /**
  * Return true if the command was successfully added.
  * Return false for a full buffer, or if the 'command' is a comment.
  */
-inline bool _enqueuecommand(const char* cmd, bool say_ok=false
+bool GCodeQueue::_enqueue(const char* cmd, bool say_ok/*=false*/
   #if NUM_SERIAL > 1
-    , int16_t port = -1
+    , int16_t pn/*=-1*/
   #endif
 ) {
-  if (*cmd == ';' || commands_in_queue >= BUFSIZE) return false;
-  strcpy(command_queue[cmd_queue_index_w], cmd);
+  if (*cmd == ';' || length >= BUFSIZE) return false;
+  strcpy(buffer[index_w], cmd);
   _commit_command(say_ok
     #if NUM_SERIAL > 1
-      , port
+      , pn
     #endif
   );
   return true;
  * Enqueue with Serial Echo
  * Return true if the command was consumed
  */
-bool enqueue_and_echo_command(const char* cmd) {
+bool GCodeQueue::enqueue_one(const char* cmd) {
 
-  //SERIAL_ECHOPGM("enqueue_and_echo_command(\"");
+  //SERIAL_ECHOPGM("enqueue_one(\"");
   //SERIAL_ECHO(cmd);
   //SERIAL_ECHOPGM("\") \n");
 
   if (*cmd == 0 || *cmd == '\n' || *cmd == '\r') return true;
 
-  if (_enqueuecommand(cmd)) {
+  if (_enqueue(cmd)) {
     SERIAL_ECHO_START();
     SERIAL_ECHOLNPAIR(MSG_ENQUEUEING, cmd, "\"");
     return true;
   return false;
 }
 
-#if HAS_QUEUE_FRONT
+/**
+ * Process the next "immediate" command.
+ */
+bool GCodeQueue::process_injected_command() {
+  if (injected_commands_P == nullptr) return false;
 
-  bool early_cmd; // = false
+  char c;
+  size_t i = 0;
+  while ((c = pgm_read_byte(&injected_commands_P[i])) && c != '\n') i++;
+  if (!i) return false;
 
-  /**
-   * Insert a high Priority command from RAM into the main command buffer.
-   * Return true if the command was consumed
-   * Return false for a full buffer, or if the 'command' is a comment.
-   */
-  inline bool _enqueuecommand_front(const char* cmd) {
-    if (*cmd == 0 || *cmd == '\n' || *cmd == '\r') return true;
-    if (*cmd == ';' || commands_in_queue >= BUFSIZE) return false;
-    if (cmd_queue_index_r == 0) cmd_queue_index_r = BUFSIZE;
-    --cmd_queue_index_r;
-    strcpy(command_queue[cmd_queue_index_r], cmd);
-    send_ok[cmd_queue_index_r] = false;
-    #if NUM_SERIAL > 1
-      command_queue_port[cmd_queue_index_r] = -1;
-    #endif
-    commands_in_queue++;
-    return true;
-  }
+  char cmd[i + 1];
+  memcpy_P(cmd, injected_commands_P, i);
+  cmd[i] = '\0';
 
-  /**
-   * Insert in the front of queue, one or many commands to run from program memory.
-   * Aborts the current queue, if any.
-   * Note: drain_injected_commands_P() must be called repeatedly to drain the commands afterwards
-   */
-  void enqueue_and_echo_commands_front_P(PGM_P const pgcode) {
-    early_cmd = true;
-    enqueue_and_echo_commands_P(pgcode);
-  }
+  injected_commands_P = c ? injected_commands_P + i + 1 : nullptr;
 
-#endif
+  parser.parse(cmd);
+  PORT_REDIRECT(SERIAL_PORT);
+  gcode.process_parsed_command();
+  PORT_RESTORE();
 
-/**
- * Inject the next "immediate" command, when possible, onto the front of the queue.
- * Return true if any immediate commands remain to inject.
- * Do not inject a comment or use leading space!.
- */
-static bool drain_injected_commands_P() {
-  while (injected_commands_P != nullptr) {
-    size_t i = 0;
-    char c, cmd[60];
-    strncpy_P(cmd, injected_commands_P, sizeof(cmd) - 1);
-    cmd[sizeof(cmd) - 1] = '\0';
-    while ((c = cmd[i]) && c != '\n') i++; // find the end of this gcode command
-    cmd[i] = '\0';
-
-    if (
-      #if HAS_QUEUE_FRONT
-        early_cmd ? _enqueuecommand_front(cmd) :
-      #endif
-      enqueue_and_echo_command(cmd)
-    ) {
-      injected_commands_P = c ? injected_commands_P + i + 1 : nullptr; // next command or done
-      #if HAS_QUEUE_FRONT
-        if (!c) early_cmd = false;
-      #endif
-    }
-    else
-      return true; // buffer is full (or command is comment);
-  }
-  return false;   // return whether any more remain
+  return true;
 }
 
 /**
  * Enqueue one or many commands to run from program memory.
+ * Do not inject a comment or use leading spaces!
  * Aborts the current queue, if any.
- * Note: drain_injected_commands_P() must be called repeatedly to drain the commands afterwards
+ * Note: process_injected_command() will be called to drain any commands afterwards
  */
-void enqueue_and_echo_commands_P(PGM_P const pgcode) {
-  #if HAS_QUEUE_FRONT
-    early_cmd = false;
-  #endif
+void GCodeQueue::inject_P(PGM_P const pgcode) {
   injected_commands_P = pgcode;
-  (void)drain_injected_commands_P(); // first command executed asap (when possible)
 }
 
-#if HAS_QUEUE_NOW
-  /**
-   * Enqueue and return only when commands are actually enqueued.
-   * Never call this from a G-code handler!
-   */
-  void enqueue_and_echo_command_now(const char* cmd) {
-    while (!enqueue_and_echo_command(cmd)) idle();
+/**
+ * Enqueue and return only when commands are actually enqueued.
+ * Never call this from a G-code handler!
+ */
+void GCodeQueue::enqueue_one_now(const char* cmd) {
+  while (!enqueue_one(cmd)) idle();
+}
+
+/**
+ * Enqueue from program memory and return only when commands are actually enqueued
+ * Never call this from a G-code handler!
+ */
+void GCodeQueue::enqueue_now_P(PGM_P const pgcode) {
+  size_t i = 0;
+  PGM_P p = pgcode;
+  for (;;) {
+    char c;
+    while ((c = p[i]) && c != '\n') i++;
+    char cmd[i + 1];
+    memcpy_P(cmd, p, i);
+    cmd[i] = '\0';
+    enqueue_one_now(cmd);
+    if (!c) break;
+    p += i + 1;
   }
-  #if HAS_LCD_QUEUE_NOW
-    /**
-     * Enqueue from program memory and return only when commands are actually enqueued
-     * Never call this from a G-code handler!
-     */
-    void enqueue_and_echo_commands_now_P(PGM_P const pgcode) {
-      enqueue_and_echo_commands_P(pgcode);
-      while (drain_injected_commands_P()) idle();
-    }
-  #endif
-#endif
+}
 
 /**
  * Send an "ok" message to the host, indicating
  *   P<int>  Planner space remaining
  *   B<int>  Block queue space remaining
  */
-void ok_to_send() {
+void GCodeQueue::ok_to_send() {
   #if NUM_SERIAL > 1
-    const int16_t port = command_queue_port[cmd_queue_index_r];
-    if (port < 0) return;
-    PORT_REDIRECT(port);
+    const int16_t p = port[index_r];
+    if (p < 0) return;
+    PORT_REDIRECT(p);
   #endif
-  if (!send_ok[cmd_queue_index_r]) return;
+  if (!send_ok[index_r]) return;
   SERIAL_ECHOPGM(MSG_OK);
   #if ENABLED(ADVANCED_OK)
-    char* p = command_queue[cmd_queue_index_r];
+    char* p = buffer[index_r];
     if (*p == 'N') {
       SERIAL_ECHO(' ');
       SERIAL_ECHO(*p++);
         SERIAL_ECHO(*p++);
     }
     SERIAL_ECHOPGM(" P"); SERIAL_ECHO(int(BLOCK_BUFFER_SIZE - planner.movesplanned() - 1));
-    SERIAL_ECHOPGM(" B"); SERIAL_ECHO(BUFSIZE - commands_in_queue);
+    SERIAL_ECHOPGM(" B"); SERIAL_ECHO(BUFSIZE - length);
   #endif
   SERIAL_EOL();
 }
  * Send a "Resend: nnn" message to the host to
  * indicate that a command needs to be re-sent.
  */
-void flush_and_request_resend() {
+void GCodeQueue::flush_and_request_resend() {
   #if NUM_SERIAL > 1
-    const int16_t port = command_queue_port[cmd_queue_index_r];
-    if (port < 0) return;
-    PORT_REDIRECT(port);
+    const int16_t p = port[index_r];
+    if (p < 0) return;
+    PORT_REDIRECT(p);
   #endif
   SERIAL_FLUSH();
   SERIAL_ECHOPGM(MSG_RESEND);
-  SERIAL_ECHOLN(gcode_LastN + 1);
+  SERIAL_ECHOLN(last_N + 1);
   ok_to_send();
 }
 
   }
 }
 
-void gcode_line_error(PGM_P const err, const int8_t port) {
-  PORT_REDIRECT(port);
-  SERIAL_ERROR_START();
-  serialprintPGM(err);
-  SERIAL_ECHOLN(gcode_LastN);
-  while (read_serial(port) != -1);           // clear out the RX buffer
-  flush_and_request_resend();
-  serial_count[port] = 0;
-}
-
 #if ENABLED(BINARY_FILE_TRANSFER)
 
   inline bool serial_data_available(const uint8_t index) {
 
 #endif // BINARY_FILE_TRANSFER
 
+void GCodeQueue::gcode_line_error(PGM_P const err, const int8_t port) {
+  PORT_REDIRECT(port);
+  SERIAL_ERROR_START();
+  serialprintPGM(err);
+  SERIAL_ECHOLN(last_N);
+  while (read_serial(port) != -1);           // clear out the RX buffer
+  flush_and_request_resend();
+  serial_count[port] = 0;
+}
+
 FORCE_INLINE bool is_M29(const char * const cmd) {  // matches "M29" & "M29 ", but not "M290", etc
   const char * const m29 = strstr_P(cmd, PSTR("M29"));
   return m29 && !NUMERIC(m29[3]);
  * Exit when the buffer is full or when no more characters are
  * left on the serial port.
  */
-inline void get_serial_commands() {
+void GCodeQueue::get_serial_commands() {
   static char serial_line_buffer[NUM_SERIAL][MAX_CMD_SIZE];
   static bool serial_comment_mode[NUM_SERIAL] = { false }
               #if ENABLED(PAREN_COMMENTS)
   #if NO_TIMEOUTS > 0
     static millis_t last_command_time = 0;
     const millis_t ms = millis();
-    if (commands_in_queue == 0 && !serial_data_available() && ELAPSED(ms, last_command_time + NO_TIMEOUTS)) {
+    if (length == 0 && !serial_data_available() && ELAPSED(ms, last_command_time + NO_TIMEOUTS)) {
       SERIAL_ECHOLNPGM(MSG_WAIT);
       last_command_time = ms;
     }
   /**
    * Loop while serial characters are incoming and the queue is not full
    */
-  while (commands_in_queue < BUFSIZE && serial_data_available()) {
+  while (length < BUFSIZE && serial_data_available()) {
     for (uint8_t i = 0; i < NUM_SERIAL; ++i) {
       int c;
       if ((c = read_serial(i)) < 0) continue;
 
           gcode_N = strtol(npos + 1, nullptr, 10);
 
-          if (gcode_N != gcode_LastN + 1 && !M110)
+          if (gcode_N != last_N + 1 && !M110)
             return gcode_line_error(PSTR(MSG_ERR_LINE_NO), i);
 
           char *apos = strrchr(command, '*');
           else
             return gcode_line_error(PSTR(MSG_ERR_NO_CHECKSUM), i);
 
-          gcode_LastN = gcode_N;
+          last_N = gcode_N;
         }
         #if ENABLED(SDSUPPORT)
           // Pronterface "M29" and "M29 " has no line number
         #endif
 
         // Add the command to the queue
-        _enqueuecommand(serial_line_buffer[i], true
+        _enqueue(serial_line_buffer[i], true
           #if NUM_SERIAL > 1
             , i
           #endif
    * or until the end of the file is reached. The special character '#'
    * can also interrupt buffering.
    */
-  inline void get_sdcard_commands() {
+  inline void GCodeQueue::get_sdcard_commands() {
     static bool stop_buffering = false,
                 sd_comment_mode = false
                 #if ENABLED(PAREN_COMMENTS)
      * due to checksums, however, no checksums are used in SD printing.
      */
 
-    if (commands_in_queue == 0) stop_buffering = false;
+    if (length == 0) stop_buffering = false;
 
     uint16_t sd_count = 0;
     bool card_eof = card.eof();
-    while (commands_in_queue < BUFSIZE && !card_eof && !stop_buffering) {
+    while (length < BUFSIZE && !card_eof && !stop_buffering) {
       const int16_t n = card.get();
       char sd_char = (char)n;
       card_eof = card.eof();
             #if ENABLED(PRINTER_EVENT_LEDS)
               printerEventLEDs.onPrintCompleted();
               #if HAS_RESUME_CONTINUE
-                enqueue_and_echo_commands_P(PSTR("M0 S"
+                inject_P(PSTR("M0 S"
                   #if HAS_LCD_MENU
                     "1800"
                   #else
         // Skip empty lines and comments
         if (!sd_count) { thermalManager.manage_heater(); continue; }
 
-        command_queue[cmd_queue_index_w][sd_count] = '\0'; // terminate string
+        buffer[index_w][sd_count] = '\0'; // terminate string
         sd_count = 0; // clear sd line buffer
 
         _commit_command(false);
           #if ENABLED(PAREN_COMMENTS)
             && ! sd_comment_paren_mode
           #endif
-        ) command_queue[cmd_queue_index_w][sd_count++] = sd_char;
+        ) buffer[index_w][sd_count++] = sd_char;
       }
     }
   }
  *  - The active serial input (usually USB)
  *  - The SD card file being actively printed
  */
-void get_available_commands() {
-
-  // if any immediate commands remain, don't get other commands yet
-  if (drain_injected_commands_P()) return;
+void GCodeQueue::get_available_commands() {
 
   get_serial_commands();
 
 /**
  * Get the next command in the queue, optionally log it to SD, then dispatch it
  */
-void advance_command_queue() {
+void GCodeQueue::advance() {
+
+  // Process immediate commands
+  if (process_injected_command()) return;
 
-  if (!commands_in_queue) return;
+  // Return if the G-code buffer is empty
+  if (!length) return;
 
   #if ENABLED(SDSUPPORT)
 
     if (card.flag.saving) {
-      char* command = command_queue[cmd_queue_index_r];
+      char* command = buffer[index_r];
       if (is_M29(command)) {
         // M29 closes the file
         card.closefile();
   #endif // SDSUPPORT
 
   // The queue may be reset by a command handler or by code invoked by idle() within a handler
-  if (commands_in_queue) {
-    --commands_in_queue;
-    if (++cmd_queue_index_r >= BUFSIZE) cmd_queue_index_r = 0;
+  if (length) {
+    --length;
+    if (++index_r >= BUFSIZE) index_r = 0;
   }
 
 }

Marlin/src/gcode/queue.h

 
 #include "../inc/MarlinConfig.h"
 
-/**
- * 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.
- */
-extern long gcode_LastN, Stopped_gcode_LastN;
-
-/**
- * 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.
- */
-extern uint8_t commands_in_queue, // Count of commands in the queue
-               cmd_queue_index_r; // Ring buffer read position
+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.
+   */
+  static long last_N, stopped_N;
 
-extern char command_queue[BUFSIZE][MAX_CMD_SIZE];
+  static inline void stop() { stopped_N = last_N; }
 
-/*
- * The port that the command was received on
- */
-#if NUM_SERIAL > 1
-  extern int16_t command_queue_port[BUFSIZE];
-#endif
+  /**
+   * 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.
+   */
+  static uint8_t length,  // Count of commands in the queue
+                 index_r; // Ring buffer read position
 
-/**
- * Initialization of queue for setup()
- */
-void queue_setup();
+  static char buffer[BUFSIZE][MAX_CMD_SIZE];
 
-/**
- * Clear the Marlin command queue
- */
-void clear_command_queue();
+  /*
+   * The port that the command was received on
+   */
+  #if NUM_SERIAL > 1
+    static int16_t port[BUFSIZE];
+  #endif
 
-/**
- * Clear the serial line and request a resend of
- * the next expected line number.
- */
-void flush_and_request_resend();
+  GCodeQueue();
 
-/**
- * Send an "ok" message to the host, indicating
- * that a command was successfully processed.
- *
- * If ADVANCED_OK is enabled also include:
- *   N<int>  Line number of the command, if any
- *   P<int>  Planner space remaining
- *   B<int>  Block queue space remaining
- */
-void ok_to_send();
+  /**
+   * Clear the Marlin command queue
+   */
+  static void clear();
 
-#if ENABLED(ADVANCED_PAUSE_FEATURE)
   /**
-   * Insert in the front of queue, one or many commands to run from program memory.
+   * Enqueue one or many commands to run from program memory.
    * Aborts the current queue, if any.
-   * Note: drain_injected_commands_P() must be called repeatedly to drain the commands afterwards
+   * Note: process_injected_command() will process them.
    */
-  void enqueue_and_echo_commands_front_P(PGM_P const pgcode);
-#endif
+  static void inject_P(PGM_P const pgcode);
 
-/**
- * Enqueue one or many commands to run from program memory.
- * Aborts the current queue, if any.
- * Note: drain_injected_commands_P() must be called repeatedly to drain the commands afterwards
- */
-void enqueue_and_echo_commands_P(PGM_P const pgcode);
+  /**
+   * Enqueue and return only when commands are actually enqueued
+   */
+  static void enqueue_one_now(const char* cmd);
 
-/**
- * Enqueue with Serial Echo
- * Return true on success
- */
-bool enqueue_and_echo_command(const char* cmd);
+  /**
+   * Enqueue from program memory and return only when commands are actually enqueued
+   */
+  static void enqueue_now_P(PGM_P const cmd);
 
-#define HAS_LCD_QUEUE_NOW (ENABLED(MALYAN_LCD) || (HAS_LCD_MENU && ANY(AUTO_BED_LEVELING_UBL, PID_AUTOTUNE_MENU, ADVANCED_PAUSE_FEATURE)))
-#define HAS_QUEUE_NOW (ENABLED(SDSUPPORT) || HAS_LCD_QUEUE_NOW)
-#define HAS_QUEUE_FRONT ENABLED(ADVANCED_PAUSE_FEATURE)
+  /**
+   * Get the next command in the queue, optionally log it to SD, then dispatch it
+   */
+  static void advance();
 
-#if HAS_QUEUE_NOW
   /**
-   * Enqueue and return only when commands are actually enqueued
+   * Add to the circular command queue the next command from:
+   *  - The command-injection queue (injected_commands_P)
+   *  - The active serial input (usually USB)
+   *  - The SD card file being actively printed
    */
-  void enqueue_and_echo_command_now(const char* cmd);
-  #if HAS_LCD_QUEUE_NOW
-    /**
-     * Enqueue from program memory and return only when commands are actually enqueued
-     */
-    void enqueue_and_echo_commands_now_P(PGM_P const cmd);
+  static void get_available_commands();
+
+  /**
+   * Send an "ok" message to the host, indicating
+   * that a command was successfully processed.
+   *
+   * If ADVANCED_OK is enabled also include:
+   *   N<int>  Line number of the command, if any
+   *   P<int>  Planner space remaining
+   *   B<int>  Block queue space remaining
+   */
+  static void ok_to_send();
+
+  /**
+   * Clear the serial line and request a resend of
+   * the next expected line number.
+   */
+  static void flush_and_request_resend();
+
+private:
+
+  static uint8_t index_w;  // Ring buffer write position
+
+  static void get_serial_commands();
+
+  #if ENABLED(SDSUPPORT)
+    static void get_sdcard_commands();
   #endif
-#endif
 
-/**
- * Add to the circular command queue the next command from:
- *  - The command-injection queue (injected_commands_P)
- *  - The active serial input (usually USB)
- *  - The SD card file being actively printed
- */
-void get_available_commands();
+  static void _commit_command(bool say_ok
+    #if NUM_SERIAL > 1
+      , int16_t p=-1
+    #endif
+  );
 
-/**
- * Get the next command in the queue, optionally log it to SD, then dispatch it
- */
-void advance_command_queue();
+  static bool _enqueue(const char* cmd, bool say_ok=false
+    #if NUM_SERIAL > 1
+      , int16_t p=-1
+    #endif
+  );
+
+  // Process the next "immediate" command
+  static bool process_injected_command();
+
+  /**
+   * Enqueue with Serial Echo
+   * Return true on success
+   */
+  static bool enqueue_one(const char* cmd);
+
+  static void gcode_line_error(PGM_P const err, const int8_t port);
+
+};
+
+extern GCodeQueue queue;

Marlin/src/gcode/sdcard/M28_M29.cpp

       SERIAL_ECHOLN(p);
       card.openFile(p, false);
       #if NUM_SERIAL > 1
-        card.transfer_port_index = command_queue_port[cmd_queue_index_r];
+        card.transfer_port_index = queue.port[queue.index_r];
       #endif
     }
     else

Marlin/src/lcd/extensible_ui/ui_api.cpp

   float getFeedrate_percent() { return feedrate_percentage; }
 
   void enqueueCommands_P(PGM_P const gcode) {
-    enqueue_and_echo_commands_P(gcode);
+    queue.inject_P(gcode);
   }
 
-  bool commandsInQueue() { return (planner.movesplanned() || commands_in_queue); }
+  bool commandsInQueue() { return (planner.movesplanned() || queue.length); }
 
   bool isAxisPositionKnown(const axis_t axis) {
     return TEST(axis_known_position, axis);

Marlin/src/lcd/malyanlcd.cpp

     case 'E':
       // enable or disable steppers
       // switch to relative
-      enqueue_and_echo_commands_now_P(PSTR("G91"));
-      enqueue_and_echo_commands_now_P(steppers_enabled ? PSTR("M18") : PSTR("M17"));
+      queue.enqueue_now_P(PSTR("G91"));
+      queue.enqueue_now_P(steppers_enabled ? PSTR("M18") : PSTR("M17"));
       steppers_enabled = !steppers_enabled;
       break;
     case 'A':
       // The M200 class UI seems to send movement in .1mm values.
       char cmd[20];
       sprintf_P(cmd, PSTR("G1 %c%03.1f"), axis, atof(command + 1) / 10.0);
-      enqueue_and_echo_command_now(cmd);
+      queue.enqueue_one_now(cmd);
     } break;
     default:
       SERIAL_ECHOLNPAIR("UNKNOWN J COMMAND", command);
             true
           #endif
         );
-        clear_command_queue();
+        queue.clear();
         quickstop_stepper();
         print_job_timer.stop();
         thermalManager.disable_all_heaters();
       break;
     case 'H':
       // Home all axis
-      enqueue_and_echo_commands_now_P(PSTR("G28"));
+      queue.enqueue_now_P(PSTR("G28"));
       break;
     default: {
       #if ENABLED(SDSUPPORT)
 
     case 'H':
       // Home all axis
-      enqueue_and_echo_command("G28");
+      queue.inject_P(PSTR("G28"));
       break;
 
     case 'L': {

Marlin/src/lcd/menu/menu.cpp

 /////////// Common Menu Actions ////////////
 ////////////////////////////////////////////
 
-void MenuItem_gcode::action(PGM_P pgcode) { enqueue_and_echo_commands_P(pgcode); }
+void MenuItem_gcode::action(PGM_P pgcode) { queue.inject_P(pgcode); }
 
 ////////////////////////////////////////////
 /////////// Menu Editing Actions ///////////
 
 #if ANY(AUTO_BED_LEVELING_UBL, PID_AUTOTUNE_MENU, ADVANCED_PAUSE_FEATURE)
 
-  void lcd_enqueue_command(const char * const cmd) {
+  void lcd_enqueue_one_now(const char * const cmd) {
     no_reentry = true;
-    enqueue_and_echo_command_now(cmd);
+    queue.enqueue_one_now(cmd);
     no_reentry = false;
   }
 
-  void lcd_enqueue_commands_P(PGM_P const cmd) {
+  void lcd_enqueue_one_now_P(PGM_P const cmd) {
     no_reentry = true;
-    enqueue_and_echo_commands_now_P(cmd);
+    queue.enqueue_now_P(cmd);
     no_reentry = false;
   }
 

Marlin/src/lcd/menu/menu.h

 #endif
 
 #if ANY(AUTO_BED_LEVELING_UBL, PID_AUTOTUNE_MENU, ADVANCED_PAUSE_FEATURE)
-  void lcd_enqueue_command(const char * const cmd);
-  void lcd_enqueue_commands_P(PGM_P const cmd);
+  void lcd_enqueue_one_now(const char * const cmd);
+  void lcd_enqueue_one_now_P(PGM_P const cmd);
 #endif
 
 #if ENABLED(LEVEL_BED_CORNERS)

Marlin/src/lcd/menu/menu_advanced.cpp

   // Set the home offset based on the current_position
   //
   void _lcd_set_home_offsets() {
-    enqueue_and_echo_commands_P(PSTR("M428"));
+    queue.inject_P(PSTR("M428"));
     ui.return_to_status();
   }
 #endif
         autotune_temp[e]
       #endif
     );
-    lcd_enqueue_command(cmd);
+    lcd_enqueue_one_now(cmd);
   }
 
 #endif // PID_AUTOTUNE_MENU

Marlin/src/lcd/menu/menu_bed_corners.cpp

   ui.defer_status_screen();
   if (!all_axes_known()) {
     set_all_unhomed();
-    enqueue_and_echo_commands_P(PSTR("G28"));
+    queue.inject_P(PSTR("G28"));
   }
 
   // Disable leveling so the planner won't mess with us

Marlin/src/lcd/menu/menu_bed_leveling.cpp

         ui.wait_for_bl_move = true;
         ui.goto_screen(_lcd_level_bed_done);
         #if ENABLED(MESH_BED_LEVELING)
-          enqueue_and_echo_commands_P(PSTR("G29 S2"));
+          queue.inject_P(PSTR("G29 S2"));
         #elif ENABLED(PROBE_MANUALLY)
-          enqueue_and_echo_commands_P(PSTR("G29 V1"));
+          queue.inject_P(PSTR("G29 V1"));
         #endif
       }
       else
     // G29 Records Z, moves, and signals when it pauses
     ui.wait_for_bl_move = true;
     #if ENABLED(MESH_BED_LEVELING)
-      enqueue_and_echo_commands_P(manual_probe_index ? PSTR("G29 S2") : PSTR("G29 S1"));
+      queue.inject_P(manual_probe_index ? PSTR("G29 S2") : PSTR("G29 S1"));
     #elif ENABLED(PROBE_MANUALLY)
-      enqueue_and_echo_commands_P(PSTR("G29 V1"));
+      queue.inject_P(PSTR("G29 V1"));
     #endif
   }
 
     ui.defer_status_screen();
     set_all_unhomed();
     ui.goto_screen(_lcd_level_bed_homing);
-    enqueue_and_echo_commands_P(PSTR("G28"));
+    queue.inject_P(PSTR("G28"));
   }
 
 #endif // PROBE_MANUALLY || MESH_BED_LEVELING

Marlin/src/lcd/menu/menu_configuration.cpp

 
     auto _recalc_offsets = []{
       if (active_extruder && all_axes_known()) {  // For the 2nd extruder re-home so the next tool-change gets the new offsets.
-        enqueue_and_echo_commands_P(PSTR("G28")); // In future, we can babystep the 2nd extruder (if active), making homing unnecessary.
+        queue.inject_P(PSTR("G28")); // In future, we can babystep the 2nd extruder (if active), making homing unnecessary.
         active_extruder = 0;
       }
     };

Marlin/src/lcd/menu/menu_custom.cpp

 #endif
 
 void _lcd_user_gcode(PGM_P const cmd) {
-  enqueue_and_echo_commands_P(cmd);
+  queue.inject_P(cmd);
   #if ENABLED(USER_SCRIPT_AUDIBLE_FEEDBACK)
     ui.completion_feedback();
   #endif

Marlin/src/lcd/menu/menu_delta_calibrate.cpp

   }
 
   void _lcd_delta_calibrate_home() {
-    enqueue_and_echo_commands_P(PSTR("G28"));
+    queue.inject_P(PSTR("G28"));
     ui.goto_screen(_lcd_calibrate_homing);
   }
 

Marlin/src/lcd/menu/menu_filament.cpp

   char cmd[11];
   sprintf_P(cmd, _change_filament_temp_command(), _change_filament_temp_extruder);
   thermalManager.setTargetHotend(temperature, _change_filament_temp_extruder);
-  lcd_enqueue_command(cmd);
+  lcd_enqueue_one_now(cmd);
 }
 inline void _lcd_change_filament_temp_1_func()    { _change_filament_temp(ui.preheat_hotend_temp[0]); }
 inline void _lcd_change_filament_temp_2_func()    { _change_filament_temp(ui.preheat_hotend_temp[1]); }

Marlin/src/lcd/menu/menu_job_recovery.cpp

 
 static void lcd_power_loss_recovery_resume() {
   ui.return_to_status();
-  enqueue_and_echo_commands_P(PSTR("M1000"));
+  queue.inject_P(PSTR("M1000"));
 }
 
 static void lcd_power_loss_recovery_cancel() {

Marlin/src/lcd/menu/menu_ubl.cpp

  * UBL Build Custom Mesh Command
  */
 void _lcd_ubl_build_custom_mesh() {
-  char UBL_LCD_GCODE[20];
-  enqueue_and_echo_commands_P(PSTR("G28"));
+  char ubl_lcd_gcode[20];
+  queue.inject_P(PSTR("G28"));
   #if HAS_HEATED_BED
-    sprintf_P(UBL_LCD_GCODE, PSTR("M190 S%i"), custom_bed_temp);
-    lcd_enqueue_command(UBL_LCD_GCODE);
+    sprintf_P(ubl_lcd_gcode, PSTR("M190 S%i"), custom_bed_temp);
+    lcd_enqueue_one_now(ubl_lcd_gcode);
   #endif
-  sprintf_P(UBL_LCD_GCODE, PSTR("M109 S%i"), custom_hotend_temp);
-  lcd_enqueue_command(UBL_LCD_GCODE);
-  enqueue_and_echo_commands_P(PSTR("G29 P1"));
+  sprintf_P(ubl_lcd_gcode, PSTR("M109 S%i"), custom_hotend_temp);
+  lcd_enqueue_one_now(ubl_lcd_gcode);
+  queue.inject_P(PSTR("G29 P1"));
 }
 
 /**
  * UBL Adjust Mesh Height Command
  */
 void _lcd_ubl_adjust_height_cmd() {
-  char UBL_LCD_GCODE[16];
+  char ubl_lcd_gcode[16];
   const int ind = ubl_height_amount > 0 ? 9 : 10;
-  strcpy_P(UBL_LCD_GCODE, PSTR("G29 P6 C -"));
-  sprintf_P(&UBL_LCD_GCODE[ind], PSTR(".%i"), ABS(ubl_height_amount));
-  lcd_enqueue_command(UBL_LCD_GCODE);
+  strcpy_P(ubl_lcd_gcode, PSTR("G29 P6 C -"));
+  sprintf_P(&ubl_lcd_gcode[ind], PSTR(".%i"), ABS(ubl_height_amount));
+  lcd_enqueue_one_now(ubl_lcd_gcode);
 }
 
 /**
  * UBL Validate Custom Mesh Command
  */
 void _lcd_ubl_validate_custom_mesh() {
-  char UBL_LCD_GCODE[24];
+  char ubl_lcd_gcode[24];
   const int temp =
     #if HAS_HEATED_BED
       custom_bed_temp
       0
     #endif
   ;
-  sprintf_P(UBL_LCD_GCODE, PSTR("G26 C B%i H%i P"), temp, custom_hotend_temp);
-  lcd_enqueue_commands_P(PSTR("G28"));
-  lcd_enqueue_command(UBL_LCD_GCODE);
+  sprintf_P(ubl_lcd_gcode, PSTR("G26 C B%i H%i P"), temp, custom_hotend_temp);
+  lcd_enqueue_one_now_P(PSTR("G28"));
+  lcd_enqueue_one_now(ubl_lcd_gcode);
 }
 
 /**
  * UBL Grid Leveling Command
  */
 void _lcd_ubl_grid_level_cmd() {
-  char UBL_LCD_GCODE[12];
-  sprintf_P(UBL_LCD_GCODE, PSTR("G29 J%i"), side_points);
-  lcd_enqueue_command(UBL_LCD_GCODE);
+  char ubl_lcd_gcode[12];
+  sprintf_P(ubl_lcd_gcode, PSTR("G29 J%i"), side_points);
+  lcd_enqueue_one_now(ubl_lcd_gcode);
 }
 
 /**
  * UBL Fill-in Amount Mesh Command
  */
 void _lcd_ubl_fillin_amount_cmd() {
-  char UBL_LCD_GCODE[18];
-  sprintf_P(UBL_LCD_GCODE, PSTR("G29 P3 R C.%i"), ubl_fillin_amount);
-  lcd_enqueue_command(UBL_LCD_GCODE);
+  char ubl_lcd_gcode[18];
+  sprintf_P(ubl_lcd_gcode, PSTR("G29 P3 R C.%i"), ubl_fillin_amount);
+  lcd_enqueue_one_now(ubl_lcd_gcode);
 }
 
 /**
  * UBL Load Mesh Command
  */
 void _lcd_ubl_load_mesh_cmd() {
-  char UBL_LCD_GCODE[25];
-  sprintf_P(UBL_LCD_GCODE, PSTR("G29 L%i"), ubl_storage_slot);
-  lcd_enqueue_command(UBL_LCD_GCODE);
-  sprintf_P(UBL_LCD_GCODE, PSTR("M117 " MSG_MESH_LOADED), ubl_storage_slot);
-  lcd_enqueue_command(UBL_LCD_GCODE);
+  char ubl_lcd_gcode[25];
+  sprintf_P(ubl_lcd_gcode, PSTR("G29 L%i"), ubl_storage_slot);
+  lcd_enqueue_one_now(ubl_lcd_gcode);
+  sprintf_P(ubl_lcd_gcode, PSTR("M117 " MSG_MESH_LOADED), ubl_storage_slot);
+  lcd_enqueue_one_now(ubl_lcd_gcode);
 }
 
 /**
  * UBL Save Mesh Command
  */
 void _lcd_ubl_save_mesh_cmd() {
-  char UBL_LCD_GCODE[25];
-  sprintf_P(UBL_LCD_GCODE, PSTR("G29 S%i"), ubl_storage_slot);
-  lcd_enqueue_command(UBL_LCD_GCODE);
-  sprintf_P(UBL_LCD_GCODE, PSTR("M117 " MSG_MESH_SAVED), ubl_storage_slot);
-  lcd_enqueue_command(UBL_LCD_GCODE);
+  char ubl_lcd_gcode[25];
+  sprintf_P(ubl_lcd_gcode, PSTR("G29 S%i"), ubl_storage_slot);
+  lcd_enqueue_one_now(ubl_lcd_gcode);
+  sprintf_P(ubl_lcd_gcode, PSTR("M117 " MSG_MESH_SAVED), ubl_storage_slot);
+  lcd_enqueue_one_now(ubl_lcd_gcode);
 }
 
 /**
  * UBL LCD "radar" map point editing
  */
 void _lcd_ubl_map_lcd_edit_cmd() {
-  char UBL_LCD_GCODE[50], str[10], str2[10];
+  char ubl_lcd_gcode[50], str[10], str2[10];
   dtostrf(pgm_read_float(&ubl._mesh_index_to_xpos[x_plot]), 0, 2, str);
   dtostrf(pgm_read_float(&ubl._mesh_index_to_ypos[y_plot]), 0, 2, str2);
-  snprintf_P(UBL_LCD_GCODE, sizeof(UBL_LCD_GCODE), PSTR("G29 P4 X%s Y%s R%i"), str, str2, n_edit_pts);
-  lcd_enqueue_command(UBL_LCD_GCODE);
+  snprintf_P(ubl_lcd_gcode, sizeof(ubl_lcd_gcode), PSTR("G29 P4 X%s Y%s R%i"), str, str2, n_edit_pts);
+  lcd_enqueue_one_now(ubl_lcd_gcode);
 }
 
 /**
 void _lcd_ubl_output_map_lcd_cmd() {
   if (!all_axes_known()) {
     set_all_unhomed();
-    enqueue_and_echo_commands_P(PSTR("G28"));
+    queue.inject_P(PSTR("G28"));
   }
   ui.goto_screen(_lcd_ubl_map_homing);
 }

Marlin/src/lcd/ultralcd.cpp

 
         #endif // HAS_LCD_MENU
 
-        if (!homed && RRK(EN_KEYPAD_F1)) enqueue_and_echo_commands_P(PSTR("G28"));
+        if (!homed && RRK(EN_KEYPAD_F1)) queue.inject_P(PSTR("G28"));
         return true;
       }
 
       #if HAS_SPI_LCD
         lcd_pause_show_message(PAUSE_MESSAGE_PAUSING, PAUSE_MODE_PAUSE_PRINT);  // Show message immediately to let user know about pause in progress
       #endif
-      enqueue_and_echo_commands_front_P(PSTR("M25 P\nM24"));
+      queue.inject_P(PSTR("M25 P\nM24"));
     #elif ENABLED(SDSUPPORT)
-      enqueue_and_echo_commands_P(PSTR("M25"));
+      queue.inject_P(PSTR("M25"));
     #elif defined(ACTION_ON_PAUSE)
       host_action_pause();
     #endif
       wait_for_heatup = wait_for_user = false;
     #endif
     #if ENABLED(SDSUPPORT)
-      if (card.isPaused()) enqueue_and_echo_commands_P(PSTR("M24"));
+      if (card.isPaused()) queue.inject_P(PSTR("M24"));
     #endif
     #ifdef ACTION_ON_RESUME
       host_action_resume();

Marlin/src/module/planner.h

       if (cleaning_buffer_counter) {
         --cleaning_buffer_counter;
         #if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
-          if (!cleaning_buffer_counter) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
+          if (!cleaning_buffer_counter) queue.inject_P(PSTR(SD_FINISHED_RELEASECOMMAND));
         #endif
       }
     }

Marlin/src/sd/cardreader.cpp

   char cmd[4 + strlen(name) + 1]; // Room for "M23 ", filename, and null
   sprintf_P(cmd, PSTR("M23 %s"), name);
   for (char *c = &cmd[4]; *c; c++) *c = tolower(*c);
-  enqueue_and_echo_command_now(cmd);
-  enqueue_and_echo_commands_P(PSTR("M24"));
+  queue.enqueue_one_now(cmd);
+  queue.inject_P(PSTR("M24"));
 }
 
 void CardReader::startFileprint() {
     #endif
 
     print_job_timer.stop();
-    if (print_job_timer.duration() > 60) enqueue_and_echo_commands_P(PSTR("M31"));
+    if (print_job_timer.duration() > 60) queue.inject_P(PSTR("M31"));
 
     #if ENABLED(SDCARD_SORT_ALPHA)
       presort();