Fix and improve G-code queue (#21122)

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

Marlin/Configuration_adv.h

@@ -2016,6 +2016,12 @@
   //#define SERIAL_STATS_DROPPED_RX
 #endif
 
+// Monitor RX buffer usage
+// Dump an error to the serial port if the serial receive buffer overflows.
+// If you see these errors, increase the RX_BUFFER_SIZE value.
+// Not supported on all platforms.
+//#define RX_BUFFER_MONITOR
+
 /**
  * Emergency Command Parser
  *

Marlin/src/MarlinCore.cpp

@@ -407,7 +407,7 @@ void startOrResumeJob() {
  */
 inline void manage_inactivity(const bool ignore_stepper_queue=false) {
 
-  if (queue.length < BUFSIZE) queue.get_available_commands();
+  queue.get_available_commands();
 
   const millis_t ms = millis();
 

Marlin/src/core/bug_on.h

@@ -0,0 +1,37 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Copyright (c) 2021 X-Ryl669 [https://blog.cyril.by]
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+// We need SERIAL_ECHOPAIR and macros.h
+#include "serial.h"
+
+#if ENABLED(POSTMORTEM_DEBUGGING)
+  // Useful macro for stopping the CPU on an unexpected condition
+  // This is used like SERIAL_ECHOPAIR, that is: a key-value call of the local variables you want
+  // to dump to the serial port before stopping the CPU.
+  #define BUG_ON(V...) do { SERIAL_ECHOPAIR(ONLY_FILENAME, __LINE__, ": "); SERIAL_ECHOLNPAIR(V); SERIAL_FLUSHTX(); *(char*)0 = 42; } while(0)
+#elif ENABLED(MARLIN_DEV_MODE)
+  // Don't stop the CPU here, but at least dump the bug on the serial port
+  #define BUG_ON(V...) do { SERIAL_ECHOPAIR(ONLY_FILENAME, __LINE__, ": BUG!\n"); SERIAL_ECHOLNPAIR(V); SERIAL_FLUSHTX(); } while(0)
+#else
+  // Release mode, let's ignore the bug
+  #define BUG_ON(V...) NOOP
+#endif

Marlin/src/core/language.h

@@ -131,6 +131,7 @@
 #define STR_WATCHDOG_FIRED                  "Watchdog timeout. Reset required."
 #define STR_ERR_KILLED                      "Printer halted. kill() called!"
 #define STR_ERR_STOPPED                     "Printer stopped due to errors. Fix the error and use M999 to restart. (Temperature is reset. Set it after restarting)"
+#define STR_ERR_SERIAL_MISMATCH             "Serial status mismatch"
 #define STR_BUSY_PROCESSING                 "busy: processing"
 #define STR_BUSY_PAUSED_FOR_USER            "busy: paused for user"
 #define STR_BUSY_PAUSED_FOR_INPUT           "busy: paused for input"

Marlin/src/core/macros.h

@@ -349,6 +349,31 @@
   #define CALL_IF_EXISTS(Return, That, Method, ...) \
     static_cast<Return>(Private::Call_ ## Method(That, ##__VA_ARGS__))
 
+  // Compile-time string manipulation
+  namespace CompileTimeString {
+    // Simple compile-time parser to find the position of the end of a string
+    constexpr const char* findStringEnd(const char *str) {
+      return *str ? findStringEnd(str + 1) : str;
+    }
+
+    // Check whether a string contains a slash
+    constexpr bool containsSlash(const char *str) {
+      return *str == '/' ? true : (*str ? containsSlash(str + 1) : false);
+    }
+    // Find the last position of the slash
+    constexpr const char* findLastSlashPos(const char* str) {
+      return *str == '/' ? (str + 1) : findLastSlashPos(str - 1);
+    }
+    // Compile-time evaluation of the last part of a file path
+    // Typically used to shorten the path to file in compiled strings
+    // CompileTimeString::baseName(__FILE__) returns "macros.h" and not /path/to/Marlin/src/core/macros.h
+    constexpr const char* baseName(const char* str) {
+      return containsSlash(str) ? findLastSlashPos(findStringEnd(str)) : str;
+    }
+  }
+
+  #define ONLY_FILENAME CompileTimeString::baseName(__FILE__)
+
 #else
 
   #define MIN_2(a,b)      ((a)<(b)?(a):(b))

Marlin/src/core/serial.cpp

@@ -52,6 +52,10 @@ PGMSTR(SP_X_LBL, " X:"); PGMSTR(SP_Y_LBL, " Y:"); PGMSTR(SP_Z_LBL, " Z:"); PGMST
   #endif
 #endif
 
+#if ENABLED(MEATPACK)
+  MeatpackSerial<decltype(_SERIAL_IMPL)> mpSerial(false, _SERIAL_IMPL);
+#endif
+
 void serialprintPGM(PGM_P str) {
   while (const char c = pgm_read_byte(str++)) SERIAL_CHAR(c);
 }

Marlin/src/core/serial.h

@@ -24,6 +24,10 @@
 #include "../inc/MarlinConfig.h"
 #include "serial_hook.h"
 
+#if ENABLED(MEATPACK)
+  #include "../feature/meatpack.h"
+#endif
+
 // Commonly-used strings in serial output
 extern const char NUL_STR[], SP_P_STR[], SP_T_STR[],
                   X_STR[], Y_STR[], Z_STR[], E_STR[],
@@ -69,12 +73,19 @@ extern uint8_t marlin_debug_flags;
     typedef MultiSerial<decltype(MYSERIAL0), TERN(HAS_ETHERNET, ConditionalSerial<decltype(MYSERIAL1)>, decltype(MYSERIAL1)), 0>      SerialOutputT;
   #endif
   extern SerialOutputT          multiSerial;
-  #define SERIAL_IMPL           multiSerial
+  #define _SERIAL_IMPL          multiSerial
 #else
   #define _PORT_REDIRECT(n,p)   NOOP
   #define _PORT_RESTORE(n)      NOOP
   #define SERIAL_ASSERT(P)      NOOP
-  #define SERIAL_IMPL           MYSERIAL0
+  #define _SERIAL_IMPL          MYSERIAL0
+#endif
+
+#if ENABLED(MEATPACK)
+  extern MeatpackSerial<decltype(_SERIAL_IMPL)> mpSerial;
+  #define SERIAL_IMPL          mpSerial
+#else
+  #define SERIAL_IMPL          _SERIAL_IMPL
 #endif
 
 #define SERIAL_OUT(WHAT, V...)  (void)SERIAL_IMPL.WHAT(V)
@@ -294,7 +305,7 @@ void serialprintPGM(PGM_P str);
 #endif
 
 #define SERIAL_ECHOPGM_P(P)         (serialprintPGM(P))
-#define SERIAL_ECHOLNPGM_P(P)       (serialprintPGM(P "\n"))
+#define SERIAL_ECHOLNPGM_P(P)       do{ serialprintPGM(P); SERIAL_EOL(); }while(0)
 
 #define SERIAL_ECHOPGM(S)           (serialprintPGM(PSTR(S)))
 #define SERIAL_ECHOLNPGM(S)         (serialprintPGM(PSTR(S "\n")))

Marlin/src/feature/meatpack.cpp

@@ -110,7 +110,7 @@ void MeatPack::handle_rx_char_inner(const uint8_t c) {
   if (TEST(state, MPConfig_Bit_Active)) {                   // Is MeatPack active?
     if (!full_char_count) {                                 // No literal characters to fetch?
       uint8_t buf[2] = { 0, 0 };
-      register const uint8_t res = unpack_chars(c, buf);    // Decode the byte into one or two characters.
+      const uint8_t res = unpack_chars(c, buf);             // Decode the byte into one or two characters.
       if (res & kFirstCharIsLiteral) {                      // The 1st character couldn't be packed.
         ++full_char_count;                                  // So the next stream byte is a full character.
         if (res & kSecondCharIsLiteral) ++full_char_count;  // The 2nd character couldn't be packed. Another stream byte is a full character.
@@ -147,9 +147,7 @@ void MeatPack::handle_output_char(const uint8_t c) {
   #if ENABLED(MP_DEBUG)
     if (chars_decoded < 1024) {
       ++chars_decoded;
-      DEBUG_ECHOPGM("RB: ");
-      MYSERIAL.print((char)c);
-      DEBUG_EOL();
+      DEBUG_ECHOLNPAIR("RB: ", AS_CHAR(c));
     }
   #endif
 }
@@ -200,7 +198,7 @@ void MeatPack::handle_rx_char(const uint8_t c, const serial_index_t serial_ind)
   }
 
   if (cmd_is_next) {                      // Were two command bytes received?
-    PORT_REDIRECT(serial_ind);
+    PORT_REDIRECT(SERIAL_PORTMASK(serial_ind));
     handle_command((MeatPack_Command)c);  // Then the byte is a MeatPack command
     cmd_is_next = false;
     return;
@@ -219,7 +217,7 @@ uint8_t MeatPack::get_result_char(char* const __restrict out) {
   if (char_out_count) {
     res = char_out_count;
     char_out_count = 0;
-    for (register uint8_t i = 0; i < res; ++i)
+    for (uint8_t i = 0; i < res; ++i)
       out[i] = (char)char_out_buf[i];
   }
   return res;

Marlin/src/feature/meatpack.h

@@ -49,6 +49,7 @@
 #pragma once
 
 #include <stdint.h>
+#include "../core/serial_hook.h"
 
 /**
  * Commands sent to MeatPack to control its behavior.
@@ -78,8 +79,6 @@ enum MeatPack_ConfigStateBits : uint8_t {
 };
 
 class MeatPack {
-private:
-  friend class GCodeQueue;
 
   // Utility definitions
   static const uint8_t kCommandByte         = 0b11111111,
@@ -99,6 +98,7 @@ class MeatPack {
                  char_out_count;  // Stores number of characters to be read out.
   static uint8_t char_out_buf[2]; // Output buffer for caching up to 2 characters
 
+public:
   // Pass in a character rx'd by SD card or serial. Automatically parses command/ctrl sequences,
   // and will control state internally.
   static void handle_rx_char(const uint8_t c, const serial_index_t serial_ind);
@@ -113,11 +113,64 @@ class MeatPack {
 
   static void reset_state();
   static void report_state();
-  static uint8_t unpacked_char(register const uint8_t in);
   static uint8_t unpack_chars(const uint8_t pk, uint8_t* __restrict const chars_out);
   static void handle_command(const MeatPack_Command c);
   static void handle_output_char(const uint8_t c);
   static void handle_rx_char_inner(const uint8_t c);
 };
 
 extern MeatPack meatpack;
+
+// Implement the MeatPack serial class so it's transparent to rest of the code
+template <typename SerialT>
+struct MeatpackSerial : public SerialBase <MeatpackSerial < SerialT >> {
+  typedef SerialBase< MeatpackSerial<SerialT> > BaseClassT;
+
+  SerialT & out;
+
+  char serialBuffer[2];
+  uint8_t charCount;
+  uint8_t readIndex;
+
+  NO_INLINE size_t write(uint8_t c) { return out.write(c); }
+  void flush()                      { out.flush();  }
+  void begin(long br)               { out.begin(br); readIndex = 0; }
+  void end()                        { out.end(); }
+
+  void msgDone()                    { out.msgDone(); }
+  // Existing instances implement Arduino's operator bool, so use that if it's available
+  bool connected()                  { return Private::HasMember_connected<SerialT>::value ? CALL_IF_EXISTS(bool, &out, connected) : (bool)out; }
+  void flushTX()                    { CALL_IF_EXISTS(void, &out, flushTX); }
+
+  int available(uint8_t index) {
+    // There is a potential issue here with multiserial, since it'll return its decoded buffer whatever the serial index here.
+    // So, instead of doing MeatpackSerial<MultiSerial<...>> we should do MultiSerial<MeatpackSerial<...>, MeatpackSerial<...>>
+    // TODO, let's fix this later on
+
+    if (charCount) return charCount;          // The buffer still has data
+    if (out.available(index) <= 0) return 0;  // No data to read
+
+    // Don't read in read method, instead do it here, so we can make progress in the read method
+    const int r = out.read(index);
+    if (r == -1) return 0;  // This is an error from the underlying serial code
+    meatpack.handle_rx_char((uint8_t)r, index);
+    charCount = meatpack.get_result_char(serialBuffer);
+    readIndex = 0;
+
+    return charCount;
+  }
+
+  int readImpl(const uint8_t index) {
+    // Not enough char to make progress?
+    if (charCount == 0 && available(index) == 0) return -1;
+
+    charCount--;
+    return serialBuffer[readIndex++];
+  }
+
+  int read(uint8_t index) { return readImpl(index); }
+  int available()         { return available(0); }
+  int read()              { return readImpl(0); }
+
+  MeatpackSerial(const bool e, SerialT & out) : BaseClassT(e), out(out) {}
+};