Patch drain_queued_commands_P

Fix a SNAFU that should have been caught in #3012

Marlin/Marlin_main.cpp

@@ -491,23 +491,20 @@ extern "C" {
  */
 static bool drain_queued_commands_P() {
   if (queued_commands_P != NULL) {
-    // Get the next gcode to run
     size_t i = 0;
-    char c;
+    char c, cmd[30];
-    while ((c = queued_commands_P[i++]) && c != '\n') { };
+    strncpy_P(cmd, queued_commands_P, sizeof(cmd) - 1);
-    if (i > 1) {
+    cmd[sizeof(cmd) - 1] = '\0';
-      char cmd[i];
+    while ((c = cmd[i]) && c != '\n') i++; // find the end of this gcode command
-      strncpy_P(cmd, queued_commands_P, i - 1);
+    cmd[i] = '\0';
-      cmd[i - 1] = '\0';
+    if (enqueue_and_echo_command(cmd)) {   // success?
-      if (enqueue_and_echo_command(cmd)) {      // buffer was not full (else we will retry later)
+      if (c)                               // newline char?
-        if (c)
+        queued_commands_P += i + 1;        // advance to the next command
-          queued_commands_P += i;     // move to next command
+      else
-        else
+        queued_commands_P = NULL;          // nul char? no more commands
-          queued_commands_P = NULL;   // no more commands in the sequence
-      }
     }
   }
-  return (queued_commands_P != NULL); // any more left to add?
+  return (queued_commands_P != NULL);      // return whether any more remain
 }
 
 /**