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

Marlin/src/gcode/queue.cpp

@@ -132,6 +132,7 @@ inline bool _enqueuecommand(const char* cmd, bool say_ok=false
 
 /**
  * Enqueue with Serial Echo
+ * Return true if the command was consumed
  */
 bool enqueue_and_echo_command(const char* cmd) {
 
@@ -139,10 +140,7 @@ 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();
@@ -152,30 +150,81 @@ bool enqueue_and_echo_command(const char* cmd) {
   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)
 }