Immediate commands take precedence

Marlin/Marlin_main.cpp

   extern void digipot_i2c_init();
 #endif
 
+inline void echo_command(char * const cmd) {
+  SERIAL_ECHO_START;
+  SERIAL_ECHOPAIR(MSG_ENQUEUEING, cmd);
+  SERIAL_CHAR('"');
+  SERIAL_EOL;
+}
+
+/**
+ * Shove a command in RAM to the front of the main command queue.
+ * Return true if the command is successfully added.
+ */
+inline bool _shovecommand(const char* cmd, bool say_ok=false) {
+  if (*cmd == ';' || commands_in_queue >= BUFSIZE) return false;
+  cmd_queue_index_r = (cmd_queue_index_r + BUFSIZE - 1) % BUFSIZE; // Index of the previous slot
+  commands_in_queue++;
+  strcpy(command_queue[cmd_queue_index_r], cmd);
+  send_ok[cmd_queue_index_r] = say_ok;
+  return true;
+}
+
+/**
+ * Shove a command to the front of the queue with Serial Echo
+ * Return true if the command is successfully added.
+ */
+bool shove_and_echo_command(const char* cmd, bool say_ok=false) {
+  if (_shovecommand(cmd, say_ok)) {
+    echo_command(cmd);
+    return true;
+  }
+  return false;
+}
+
 /**
- * Inject the next "immediate" command, when possible.
+ * Shove a command onto the front of the queue,
+ * and don't return until successful.
+ */
+void shove_and_echo_command_now(const char* cmd) {
+  while (!shove_and_echo_command(cmd)) idle();
+}
+
+/**
+ * Inject the next "immediate" command, when possible, onto the front of the queue.
  * Return true if any immediate commands remain to inject.
  */
 static bool drain_injected_commands_P() {
     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 (shove_and_echo_command(cmd)) {     // success?
       if (c)                               // newline char?
-        injected_commands_P += i + 1;        // advance to the next command
+        injected_commands_P += i + 1;      // advance to the next command
       else
-        injected_commands_P = NULL;          // nul char? no more commands
+        injected_commands_P = NULL;        // nul char? no more commands
     }
   }
-  return (injected_commands_P != NULL);      // return whether any more remain
+  return (injected_commands_P != NULL);    // return whether any more remain
 }
 
 /**
   drain_injected_commands_P(); // first command executed asap (when possible)
 }
 
+/**
+ * Clear the Marlin command queue
+ */
 void clear_command_queue() {
   cmd_queue_index_r = cmd_queue_index_w;
   commands_in_queue = 0;
 }
 
 /**
- * Copy a command directly into the main command buffer, from RAM.
- * Returns true if successfully adds the command
+ * Copy a command from RAM into the main command buffer.
+ * 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) {
   if (*cmd == ';' || commands_in_queue >= BUFSIZE) return false;
   return true;
 }
 
-void enqueue_and_echo_command_now(const char* cmd) {
-  while (!enqueue_and_echo_command(cmd)) idle();
-}
-
 /**
  * Enqueue with Serial Echo
  */
 bool enqueue_and_echo_command(const char* cmd, bool say_ok/*=false*/) {
   if (_enqueuecommand(cmd, say_ok)) {
-    SERIAL_ECHO_START;
-    SERIAL_ECHOPAIR(MSG_Enqueueing, cmd);
-    SERIAL_CHAR('"');
-    SERIAL_EOL;
+    echo_command(cmd);
     return true;
   }
   return false;
 }
 
+void enqueue_and_echo_command_now(const char* cmd) {
+  while (!enqueue_and_echo_command(cmd)) idle();
+}
+
 void setup_killpin() {
   #if HAS_KILL
     SET_INPUT_PULLUP(KILL_PIN);
 
 #endif
 
-// Set home pin
 void setup_homepin(void) {
   #if HAS_HOME
     SET_INPUT_PULLUP(HOME_PIN);
   serial_count = 0;
 }
 
+/**
+ * Get all commands waiting on the serial port and queue them.
+ * Exit when the buffer is full or when no more characters are
+ * left on the serial port.
+ */
 inline void get_serial_commands() {
   static char serial_line_buffer[MAX_CMD_SIZE];
   static bool serial_comment_mode = false;
 
 #if ENABLED(SDSUPPORT)
 
+  /**
+   * Get commands from the SD Card until the command buffer is full
+   * or until the end of the file is reached. The special character '#'
+   * can also interrupt buffering.
+   */
   inline void get_sdcard_commands() {
     static bool stop_buffering = false,
                 sd_comment_mode = false;
     uint16_t sd_count = 0;
     bool card_eof = card.eof();
     while (commands_in_queue < BUFSIZE && !card_eof && !stop_buffering) {
-      int16_t n = card.get();
+      const int16_t n = card.get();
       char sd_char = (char)n;
       card_eof = card.eof();
       if (card_eof || n == -1
         }
         if (sd_char == '#') stop_buffering = true;
 
-        sd_comment_mode = false; //for new command
+        sd_comment_mode = false; // for new command
 
-        if (!sd_count) continue; //skip empty lines
+        if (!sd_count) continue; // skip empty lines (and comment lines)
 
-        command_queue[cmd_queue_index_w][sd_count] = '\0'; //terminate string
-        sd_count = 0; //clear buffer
+        command_queue[cmd_queue_index_w][sd_count] = '\0'; // terminate string
+        sd_count = 0; // clear sd line buffer
 
         _commit_command(false);
       }

Marlin/language.h

 
 // Serial Console Messages (do not translate those!)
 
-#define MSG_Enqueueing                      "enqueueing \""
+#define MSG_ENQUEUEING                      "enqueueing \""
 #define MSG_POWERUP                         "PowerUp"
 #define MSG_EXTERNAL_RESET                  " External Reset"
 #define MSG_BROWNOUT_RESET                  " Brown out Reset"