Fix and improve G-code queue (#21122)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>

Marlin/src/gcode/queue.h

@@ -31,41 +31,84 @@
 class GCodeQueue {
 public:
   /**
-   * GCode line number handling. Hosts may include line numbers when sending
-   * commands to Marlin, and lines will be checked for sequentiality.
-   * M110 N<int> sets the current line number.
+   * The buffers per serial port.
    */
+  struct SerialState {
+    /**
+     * GCode line number handling. Hosts may include line numbers when sending
+     * commands to Marlin, and lines will be checked for sequentiality.
+     * M110 N<int> sets the current line number.
+     */
+    long last_N;
+    int count;                        //!< Number of characters read in the current line of serial input
+    char line_buffer[MAX_CMD_SIZE];   //!< The current line accumulator
+    uint8_t input_state;              //!< The input state
+  };
 
-  static long last_N[NUM_SERIAL];
+  static SerialState serial_state[NUM_SERIAL]; //!< Serial states for each serial port
 
   /**
    * GCode Command Queue
-   * A simple ring buffer of BUFSIZE command strings.
+   * A simple (circular) ring buffer of BUFSIZE command strings.
    *
    * Commands are copied into this buffer by the command injectors
    * (immediate, serial, sd card) and they are processed sequentially by
    * the main loop. The gcode.process_next_command method parses the next
    * command and hands off execution to individual handler functions.
    */
-  static uint8_t length,  // Count of commands in the queue
-                 index_r; // Ring buffer read position
-
-  static char command_buffer[BUFSIZE][MAX_CMD_SIZE];
+  struct CommandLine {
+    char buffer[MAX_CMD_SIZE];                    //!< The command buffer
+    bool skip_ok;                                 //!< Skip sending ok when command is processed?
+    TERN_(HAS_MULTI_SERIAL, serial_index_t port); //!< Serial port the command was received on
+  };
 
   /**
-   * The port that the command was received on
+   * A handy ring buffer type
    */
-  #if HAS_MULTI_SERIAL
-    static serial_index_t port[BUFSIZE];
-  #endif
-  static inline serial_index_t command_port() { return TERN0(HAS_MULTI_SERIAL, port[index_r]); }
+  struct RingBuffer {
+    uint8_t length,                 //!< Number of commands in the queue
+            index_r,                //!< Ring buffer's read position
+            index_w;                //!< Ring buffer's write position
+    CommandLine commands[BUFSIZE];  //!< The ring buffer of commands
 
-  GCodeQueue();
+    inline serial_index_t command_port() const { return TERN0(HAS_MULTI_SERIAL, commands[index_r].port); }
+
+    inline void clear() { length = index_r = index_w = 0; }
+
+    void advance_pos(uint8_t &p, const int inc) { if (++p >= BUFSIZE) p = 0; length += inc; }
+
+    void commit_command(bool skip_ok
+      #if HAS_MULTI_SERIAL
+        , serial_index_t serial_ind=-1
+      #endif
+    );
+
+    bool enqueue(const char* cmd, bool skip_ok = true
+      #if HAS_MULTI_SERIAL
+        , serial_index_t serial_ind=-1
+      #endif
+    );
+
+    void ok_to_send();
+
+    inline bool full(uint8_t cmdCount=1) const { return length > (BUFSIZE - cmdCount); }
+
+    inline bool empty() const { return length == 0; }
+
+    inline CommandLine& peek_next_command() { return commands[index_r]; }
+
+    inline char* peek_next_command_string() { return peek_next_command().buffer; }
+  };
+
+  /**
+   * The ring buffer of commands
+   */
+  static RingBuffer ring_buffer;
 
   /**
    * Clear the Marlin command queue
    */
-  static void clear();
+  static void clear() { ring_buffer.clear(); }
 
   /**
    * Next Injected Command (PROGMEM) pointer. (nullptr == empty)
@@ -112,7 +155,7 @@ public:
   /**
    * Check whether there are any commands yet to be executed
    */
-  static bool has_commands_queued();
+  static bool has_commands_queued() { return ring_buffer.length || injected_commands_P || injected_commands[0]; }
 
   /**
    * Get the next command in the queue, optionally log it to SD, then dispatch it
@@ -136,7 +179,7 @@ public:
    *   P<int>  Planner space remaining
    *   B<int>  Block queue space remaining
    */
-  static void ok_to_send();
+  static inline void ok_to_send() { ring_buffer.ok_to_send(); }
 
   /**
    * Clear the serial line and request a resend of
@@ -144,9 +187,12 @@ public:
    */
   static void flush_and_request_resend();
 
-private:
+  /**
+   * (Re)Set the current line number for the last received command
+   */
+  static inline void set_current_line_number(long n) { serial_state[ring_buffer.command_port()].last_N = n; }
 
-  static uint8_t index_w;  // Ring buffer write position
+private:
 
   static void get_serial_commands();
 
@@ -154,18 +200,6 @@ private:
     static void get_sdcard_commands();
   #endif
 
-  static void _commit_command(bool say_ok
-    #if HAS_MULTI_SERIAL
-      , serial_index_t serial_ind=-1
-    #endif
-  );
-
-  static bool _enqueue(const char* cmd, bool say_ok=false
-    #if HAS_MULTI_SERIAL
-      , serial_index_t serial_ind=-1
-    #endif
-  );
-
   // Process the next "immediate" command (PROGMEM)
   static bool process_injected_command_P();
 
@@ -180,6 +214,7 @@ private:
 
   static void gcode_line_error(PGM_P const err, const serial_index_t serial_ind);
 
+  friend class GcodeSuite;
 };
 
 extern GCodeQueue queue;