Ability to insert G-code in front of queue (#14229)

Marlin/src/Marlin.cpp

     #endif // HOST_ACTION_COMMANDS
 
     if (run_runout_script)
-      enqueue_and_echo_commands_P(PSTR(FILAMENT_RUNOUT_SCRIPT));
+      enqueue_and_echo_commands_front_P(PSTR(FILAMENT_RUNOUT_SCRIPT));
   }
 
 #endif // HAS_FILAMENT_SENSOR

Marlin/src/gcode/queue.cpp

 
 /**
  * Enqueue with Serial Echo
+ * Return true if the command was consumed
  */
 bool enqueue_and_echo_command(const char* cmd) {
 
   //SERIAL_ECHO(cmd);
   //SERIAL_ECHOPGM("\") \n");
 
-  if (*cmd == 0 || *cmd == '\n' || *cmd == '\r') {
-    //SERIAL_ECHOLNPGM("Null command found...   Did not queue!");
-    return true;
-  }
+  if (*cmd == 0 || *cmd == '\n' || *cmd == '\r') return true;
 
   if (_enqueuecommand(cmd)) {
     SERIAL_ECHO_START();
   return false;
 }
 
+#if HAS_QUEUE_FRONT
+
+  bool early_cmd; // = 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;
+  }
+
+  /**
+   * 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);
+  }
+
+#endif
+
 /**
  * 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() {
-  if (injected_commands_P != nullptr) {
+  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 (enqueue_and_echo_command(cmd))     // success?
+
+    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 (injected_commands_P != nullptr);    // return whether any more remain
+  return false;   // return whether any more remain
 }
 
 /**
- * Record 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
  */
 void enqueue_and_echo_commands_P(PGM_P const pgcode) {
+  #if HAS_QUEUE_FRONT
+    early_cmd = false;
+  #endif
   injected_commands_P = pgcode;
   (void)drain_injected_commands_P(); // first command executed asap (when possible)
 }

Marlin/src/gcode/queue.h

  */
 void ok_to_send();
 
+#if ENABLED(ADVANCED_PAUSE_FEATURE)
+  /**
+   * 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);
+#endif
+
 /**
- * Record 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
  */
 
 /**
  * Enqueue with Serial Echo
+ * Return true on success
  */
 bool enqueue_and_echo_command(const char* 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)
 
 #if HAS_QUEUE_NOW
   /**

Marlin/src/lcd/ultralcd.cpp

       #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_P(PSTR("M25 P\nM24"));
+      enqueue_and_echo_commands_front_P(PSTR("M25 P\nM24"));
     #elif ENABLED(SDSUPPORT)
       enqueue_and_echo_commands_P(PSTR("M25"));
     #elif defined(ACTION_ON_PAUSE)