Fix broken enqueue_P

Marlin/src/gcode/queue.cpp

@@ -209,6 +209,21 @@ void GCodeQueue::inject_P(PGM_P const pgcode) { injected_commands_P = pgcode; }
  */
 void GCodeQueue::enqueue_one_now(const char* cmd) { while (!enqueue_one(cmd)) idle(); }
 
+/**
+ * Attempt to enqueue a single G-code command
+ * and return 'true' if successful.
+ */
+bool GCodeQueue::enqueue_one_P(PGM_P const pgcode) {
+  size_t i = 0;
+  PGM_P p = pgcode;
+  char c;
+  while ((c = pgm_read_byte(&p[i])) && c != '\n') i++;
+  char cmd[i + 1];
+  memcpy_P(cmd, p, i);
+  cmd[i] = '\0';
+  return _enqueue(cmd);
+}
+
 /**
  * Enqueue from program memory and return only when commands are actually enqueued
  * Never call this from a G-code handler!

Marlin/src/gcode/queue.h

@@ -77,6 +77,12 @@ public:
    */
   static void enqueue_one_now(const char* cmd);
 
+  /**
+   * Attempt to enqueue a single G-code command
+   * and return 'true' if successful.
+   */
+  static bool enqueue_one_P(PGM_P const pgcode);
+
   /**
    * Enqueue from program memory and return only when commands are actually enqueued
    */
@@ -117,12 +123,6 @@ public:
    */
   static void flush_and_request_resend();
 
-  /**
-   * Attempt to enqueue a single G-code command
-   * and return 'true' if successful.
-   */
-  FORCE_INLINE static bool enqueue_P(const char* cmd) { return _enqueue(cmd); }
-
 private:
 
   static uint8_t index_w;  // Ring buffer write position