From d31a90e6a3d8ced667e06e8cfa38907432eb25d2 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Mon, 10 Feb 2020 16:33:21 -0600 Subject: [PATCH] Fix out-of-order M0 after SD printing Fixes #14774 Co-Authored-By: tol2cj --- Marlin/src/gcode/queue.cpp | 429 +++++++++++++++++++------------------ 1 file changed, 216 insertions(+), 213 deletions(-) diff --git a/Marlin/src/gcode/queue.cpp b/Marlin/src/gcode/queue.cpp index d87166d979df..a15987b5defe 100644 --- a/Marlin/src/gcode/queue.cpp +++ b/Marlin/src/gcode/queue.cpp @@ -1,6 +1,6 @@ /** * Marlin 3D Printer Firmware - * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * * Based on Sprinter and grbl. * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm @@ -44,7 +44,7 @@ GCodeQueue queue; #endif #if ENABLED(POWER_LOSS_RECOVERY) - #include "../feature/powerloss.h" + #include "../feature/power_loss_recovery.h" #endif /** @@ -52,7 +52,7 @@ GCodeQueue queue; * sending commands to Marlin, and lines will be checked for sequentiality. * M110 N sets the current line number. */ -long GCodeQueue::last_N[NUM_SERIAL]; +long gcode_N, GCodeQueue::last_N, GCodeQueue::stopped_N = 0; /** * GCode Command Queue @@ -72,7 +72,7 @@ char GCodeQueue::command_buffer[BUFSIZE][MAX_CMD_SIZE]; /* * The port that the command was received on */ -#if HAS_MULTI_SERIAL +#if NUM_SERIAL > 1 int16_t GCodeQueue::port[BUFSIZE]; #endif @@ -86,26 +86,22 @@ static int serial_count[NUM_SERIAL] = { 0 }; bool send_ok[BUFSIZE]; /** - * Next Injected PROGMEM Command pointer. (nullptr == empty) - * Internal commands are enqueued ahead of serial / SD commands. + * Next Injected Command pointer. nullptr if no commands are being injected. + * Used by Marlin internally to ensure that commands initiated from within + * are enqueued ahead of any pending serial or sd card commands. */ -PGM_P GCodeQueue::injected_commands_P; // = nullptr - -/** - * Injected SRAM Commands - */ -char GCodeQueue::injected_commands[64]; // = { 0 } +static PGM_P injected_commands_P = nullptr; GCodeQueue::GCodeQueue() { // Send "ok" after commands by default - LOOP_L_N(i, COUNT(send_ok)) send_ok[i] = true; + for (uint8_t i = 0; i < COUNT(send_ok); i++) send_ok[i] = true; } /** * Check whether there are any commands yet to be executed */ bool GCodeQueue::has_commands_queued() { - return queue.length || injected_commands_P || injected_commands[0]; + return queue.length || injected_commands_P; } /** @@ -119,13 +115,17 @@ void GCodeQueue::clear() { * Once a new command is in the ring buffer, call this to commit it */ void GCodeQueue::_commit_command(bool say_ok - #if HAS_MULTI_SERIAL + #if NUM_SERIAL > 1 , int16_t p/*=-1*/ #endif ) { send_ok[index_w] = say_ok; - TERN_(HAS_MULTI_SERIAL, port[index_w] = p); - TERN_(POWER_LOSS_RECOVERY, recovery.commit_sdpos(index_w)); + #if NUM_SERIAL > 1 + port[index_w] = p; + #endif + #if ENABLED(POWER_LOSS_RECOVERY) + recovery.commit_sdpos(index_w); + #endif if (++index_w >= BUFSIZE) index_w = 0; length++; } @@ -136,22 +136,20 @@ void GCodeQueue::_commit_command(bool say_ok * Return false for a full buffer, or if the 'command' is a comment. */ bool GCodeQueue::_enqueue(const char* cmd, bool say_ok/*=false*/ - #if HAS_MULTI_SERIAL + #if NUM_SERIAL > 1 , int16_t pn/*=-1*/ #endif ) { if (*cmd == ';' || length >= BUFSIZE) return false; strcpy(command_buffer[index_w], cmd); _commit_command(say_ok - #if HAS_MULTI_SERIAL + #if NUM_SERIAL > 1 , pn #endif ); return true; } -#define ISEOL(C) ((C) == '\n' || (C) == '\r') - /** * Enqueue with Serial Echo * Return true if the command was consumed @@ -162,20 +160,22 @@ bool GCodeQueue::enqueue_one(const char* cmd) { //SERIAL_ECHO(cmd); //SERIAL_ECHOPGM("\") \n"); - if (*cmd == 0 || ISEOL(*cmd)) return true; + if (*cmd == 0 || *cmd == '\n' || *cmd == '\r') return true; if (_enqueue(cmd)) { - SERIAL_ECHO_MSG(STR_ENQUEUEING, cmd, "\""); + SERIAL_ECHO_START(); + SERIAL_ECHOLNPAIR(MSG_ENQUEUEING, cmd, "\""); return true; } return false; } /** - * Process the next "immediate" command from PROGMEM. - * Return 'true' if any commands were processed. + * Process the next "immediate" command. + * Return 'true' if any commands were processed, + * or remain to process. */ -bool GCodeQueue::process_injected_command_P() { +bool GCodeQueue::process_injected_command() { if (injected_commands_P == nullptr) return false; char c; @@ -197,32 +197,12 @@ bool GCodeQueue::process_injected_command_P() { } /** - * Process the next "immediate" command from SRAM. - * Return 'true' if any commands were processed. + * Enqueue one or many commands to run from program memory. + * Do not inject a comment or use leading spaces! + * Aborts the current queue, if any. + * Note: process_injected_command() will be called to drain any commands afterwards */ -bool GCodeQueue::process_injected_command() { - if (injected_commands[0] == '\0') return false; - - char c; - size_t i = 0; - while ((c = injected_commands[i]) && c != '\n') i++; - - // Execute a non-blank command - if (i) { - injected_commands[i] = '\0'; - parser.parse(injected_commands); - gcode.process_parsed_command(); - } - - // Copy the next command into place - for ( - uint8_t d = 0, s = i + !!c; // dst, src - (injected_commands[d] = injected_commands[s]); // copy, exit if 0 - d++, s++ // next dst, src - ); - - return true; -} +void GCodeQueue::inject_P(PGM_P const pgcode) { injected_commands_P = pgcode; } /** * Enqueue and return only when commands are actually enqueued. @@ -230,21 +210,6 @@ bool GCodeQueue::process_injected_command() { */ 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! @@ -274,13 +239,13 @@ void GCodeQueue::enqueue_now_P(PGM_P const pgcode) { * B Block queue space remaining */ void GCodeQueue::ok_to_send() { - #if HAS_MULTI_SERIAL - const int16_t pn = command_port(); + #if NUM_SERIAL > 1 + const int16_t pn = port[index_r]; if (pn < 0) return; PORT_REDIRECT(pn); // Reply to the serial port that sent the command #endif if (!send_ok[index_r]) return; - SERIAL_ECHOPGM(STR_OK); + SERIAL_ECHOPGM(MSG_OK); #if ENABLED(ADVANCED_OK) char* p = command_buffer[index_r]; if (*p == 'N') { @@ -289,8 +254,8 @@ void GCodeQueue::ok_to_send() { while (NUMERIC_SIGNED(*p)) SERIAL_ECHO(*p++); } - SERIAL_ECHOPAIR_P(SP_P_STR, int(planner.moves_free())); - SERIAL_ECHOPAIR(" B", int(BUFSIZE - length)); + SERIAL_ECHOPGM(" P"); SERIAL_ECHO(int(BLOCK_BUFFER_SIZE - planner.movesplanned() - 1)); + SERIAL_ECHOPGM(" B"); SERIAL_ECHO(BUFSIZE - length); #endif SERIAL_EOL(); } @@ -300,25 +265,30 @@ void GCodeQueue::ok_to_send() { * indicate that a command needs to be re-sent. */ void GCodeQueue::flush_and_request_resend() { - const int16_t pn = command_port(); - #if HAS_MULTI_SERIAL + #if NUM_SERIAL > 1 + const int16_t pn = port[index_r]; if (pn < 0) return; PORT_REDIRECT(pn); // Reply to the serial port that sent the command #endif SERIAL_FLUSH(); - SERIAL_ECHOPGM(STR_RESEND); - SERIAL_ECHOLN(last_N[pn] + 1); + SERIAL_ECHOPGM(MSG_RESEND); + SERIAL_ECHOLN(last_N + 1); ok_to_send(); } inline bool serial_data_available() { - return MYSERIAL0.available() || TERN0(HAS_MULTI_SERIAL, MYSERIAL1.available()); + return false + || MYSERIAL0.available() + #if NUM_SERIAL > 1 + || MYSERIAL1.available() + #endif + ; } inline int read_serial(const uint8_t index) { switch (index) { case 0: return MYSERIAL0.read(); - #if HAS_MULTI_SERIAL + #if NUM_SERIAL > 1 case 1: return MYSERIAL1.read(); #endif default: return -1; @@ -329,7 +299,7 @@ void GCodeQueue::gcode_line_error(PGM_P const err, const int8_t pn) { PORT_REDIRECT(pn); // Reply to the serial port that sent the command SERIAL_ERROR_START(); serialprintPGM(err); - SERIAL_ECHOLN(last_N[pn]); + SERIAL_ECHOLN(last_N); while (read_serial(pn) != -1); // Clear out the RX buffer flush_and_request_resend(); serial_count[pn] = 0; @@ -340,76 +310,6 @@ FORCE_INLINE bool is_M29(const char * const cmd) { // matches "M29" & "M29 ", b return m29 && !NUMERIC(m29[3]); } -#define PS_NORMAL 0 -#define PS_EOL 1 -#define PS_QUOTED 2 -#define PS_PAREN 3 -#define PS_ESC 4 - -inline void process_stream_char(const char c, uint8_t &sis, char (&buff)[MAX_CMD_SIZE], int &ind) { - - if (sis == PS_EOL) return; // EOL comment or overflow - - #if ENABLED(PAREN_COMMENTS) - else if (sis == PS_PAREN) { // Inline comment - if (c == ')') sis = PS_NORMAL; - return; - } - #endif - - else if (sis >= PS_ESC) // End escaped char - sis -= PS_ESC; - - else if (c == '\\') { // Start escaped char - sis += PS_ESC; - if (sis == PS_ESC) return; // Keep if quoting - } - - #if ENABLED(GCODE_QUOTED_STRINGS) - - else if (sis == PS_QUOTED) { - if (c == '"') sis = PS_NORMAL; // End quoted string - } - else if (c == '"') // Start quoted string - sis = PS_QUOTED; - - #endif - - else if (c == ';') { // Start end-of-line comment - sis = PS_EOL; - return; - } - - #if ENABLED(PAREN_COMMENTS) - else if (c == '(') { // Start inline comment - sis = PS_PAREN; - return; - } - #endif - - // Backspace erases previous characters - if (c == 0x08) { - if (ind) buff[--ind] = '\0'; - } - else { - buff[ind++] = c; - if (ind >= MAX_CMD_SIZE - 1) - sis = PS_EOL; // Skip the rest on overflow - } -} - -/** - * Handle a line being completed. For an empty line - * keep sensor readings going and watchdog alive. - */ -inline bool process_line_done(uint8_t &sis, char (&buff)[MAX_CMD_SIZE], int &ind) { - sis = PS_NORMAL; - buff[ind] = 0; - if (ind) { ind = 0; return false; } - thermalManager.manage_heater(); - return true; -} - /** * Get all commands waiting on the serial port and queue them. * Exit when the buffer is full or when no more characters are @@ -417,8 +317,11 @@ inline bool process_line_done(uint8_t &sis, char (&buff)[MAX_CMD_SIZE], int &ind */ void GCodeQueue::get_serial_commands() { static char serial_line_buffer[NUM_SERIAL][MAX_CMD_SIZE]; - - static uint8_t serial_input_state[NUM_SERIAL] = { PS_NORMAL }; + static bool serial_comment_mode[NUM_SERIAL] = { false } + #if ENABLED(PAREN_COMMENTS) + , serial_comment_paren_mode[NUM_SERIAL] = { false } + #endif + ; #if ENABLED(BINARY_FILE_TRANSFER) if (card.flag.binary_mode) { @@ -438,7 +341,7 @@ void GCodeQueue::get_serial_commands() { static millis_t last_command_time = 0; const millis_t ms = millis(); if (length == 0 && !serial_data_available() && ELAPSED(ms, last_command_time + NO_TIMEOUTS)) { - SERIAL_ECHOLNPGM(STR_WAIT); + SERIAL_ECHOLNPGM(MSG_WAIT); last_command_time = ms; } #endif @@ -447,22 +350,32 @@ void GCodeQueue::get_serial_commands() { * Loop while serial characters are incoming and the queue is not full */ while (length < BUFSIZE && serial_data_available()) { - LOOP_L_N(i, NUM_SERIAL) { + for (uint8_t i = 0; i < NUM_SERIAL; ++i) { + int c; + if ((c = read_serial(i)) < 0) continue; + + char serial_char = c; - const int c = read_serial(i); - if (c < 0) continue; + /** + * If the character ends the line + */ + if (serial_char == '\n' || serial_char == '\r') { - const char serial_char = c; + // Start with comment mode off + serial_comment_mode[i] = false; + #if ENABLED(PAREN_COMMENTS) + serial_comment_paren_mode[i] = false; + #endif - if (ISEOL(serial_char)) { + // Skip empty lines and comments + if (!serial_count[i]) { thermalManager.manage_heater(); continue; } - // Reset our state, continue if the line was empty - if (process_line_done(serial_input_state[i], serial_line_buffer[i], serial_count[i])) - continue; + serial_line_buffer[i][serial_count[i]] = 0; // Terminate string + serial_count[i] = 0; // Reset buffer char* command = serial_line_buffer[i]; - while (*command == ' ') command++; // Skip leading spaces + while (*command == ' ') command++; // Skip leading spaces char *npos = (*command == 'N') ? command : nullptr; // Require the N parameter to start the line if (npos) { @@ -474,46 +387,44 @@ void GCodeQueue::get_serial_commands() { if (n2pos) npos = n2pos; } - const long gcode_N = strtol(npos + 1, nullptr, 10); + gcode_N = strtol(npos + 1, nullptr, 10); - if (gcode_N != last_N[i] + 1 && !M110) - return gcode_line_error(PSTR(STR_ERR_LINE_NO), i); + if (gcode_N != last_N + 1 && !M110) + return gcode_line_error(PSTR(MSG_ERR_LINE_NO), i); char *apos = strrchr(command, '*'); if (apos) { uint8_t checksum = 0, count = uint8_t(apos - command); while (count) checksum ^= command[--count]; if (strtol(apos + 1, nullptr, 10) != checksum) - return gcode_line_error(PSTR(STR_ERR_CHECKSUM_MISMATCH), i); + return gcode_line_error(PSTR(MSG_ERR_CHECKSUM_MISMATCH), i); } else - return gcode_line_error(PSTR(STR_ERR_NO_CHECKSUM), i); + return gcode_line_error(PSTR(MSG_ERR_NO_CHECKSUM), i); - last_N[i] = gcode_N; + last_N = gcode_N; } #if ENABLED(SDSUPPORT) // Pronterface "M29" and "M29 " has no line number else if (card.flag.saving && !is_M29(command)) - return gcode_line_error(PSTR(STR_ERR_NO_CHECKSUM), i); + return gcode_line_error(PSTR(MSG_ERR_NO_CHECKSUM), i); #endif - // - // Movement commands give an alert when the machine is stopped - // - + // Movement commands alert when stopped if (IsStopped()) { char* gpos = strchr(command, 'G'); if (gpos) { switch (strtol(gpos + 1, nullptr, 10)) { - case 0: case 1: + case 0: + case 1: #if ENABLED(ARC_SUPPORT) - case 2: case 3: + case 2: + case 3: #endif #if ENABLED(BEZIER_CURVE_SUPPORT) case 5: #endif - PORT_REDIRECT(i); // Reply to the serial port that sent the command - SERIAL_ECHOLNPGM(STR_ERR_STOPPED); + SERIAL_ECHOLNPGM(MSG_ERR_STOPPED); LCD_MESSAGEPGM(MSG_STOPPED); break; } @@ -522,12 +433,14 @@ void GCodeQueue::get_serial_commands() { #if DISABLED(EMERGENCY_PARSER) // Process critical commands early - if (strcmp_P(command, PSTR("M108")) == 0) { + if (strcmp(command, "M108") == 0) { wait_for_heatup = false; - TERN_(HAS_LCD_MENU, wait_for_user = false); + #if HAS_LCD_MENU + wait_for_user = false; + #endif } - if (strcmp_P(command, PSTR("M112")) == 0) kill(M112_KILL_STR, nullptr, true); - if (strcmp_P(command, PSTR("M410")) == 0) quickstop_stepper(); + if (strcmp(command, "M112") == 0) kill(M112_KILL_STR, nullptr, true); + if (strcmp(command, "M410") == 0) quickstop_stepper(); #endif #if defined(NO_TIMEOUTS) && NO_TIMEOUTS > 0 @@ -536,14 +449,36 @@ void GCodeQueue::get_serial_commands() { // Add the command to the queue _enqueue(serial_line_buffer[i], true - #if HAS_MULTI_SERIAL + #if NUM_SERIAL > 1 , i #endif ); } - else - process_stream_char(serial_char, serial_input_state[i], serial_line_buffer[i], serial_count[i]); - + else if (serial_count[i] >= MAX_CMD_SIZE - 1) { + // Keep fetching, but ignore normal characters beyond the max length + // The command will be injected when EOL is reached + } + else if (serial_char == '\\') { // Handle escapes + // if we have one more character, copy it over + if ((c = read_serial(i)) >= 0 && !serial_comment_mode[i] + #if ENABLED(PAREN_COMMENTS) + && !serial_comment_paren_mode[i] + #endif + ) + serial_line_buffer[i][serial_count[i]++] = (char)c; + } + else { // it's not a newline, carriage return or escape char + if (serial_char == ';') serial_comment_mode[i] = true; + #if ENABLED(PAREN_COMMENTS) + else if (serial_char == '(') serial_comment_paren_mode[i] = true; + else if (serial_char == ')') serial_comment_paren_mode[i] = false; + #endif + else if (!serial_comment_mode[i] + #if ENABLED(PAREN_COMMENTS) + && ! serial_comment_paren_mode[i] + #endif + ) serial_line_buffer[i][serial_count[i]++] = serial_char; + } } // for NUM_SERIAL } // queue has space, serial has data } @@ -551,41 +486,105 @@ void GCodeQueue::get_serial_commands() { #if ENABLED(SDSUPPORT) /** - * Get lines from the SD Card until the command buffer is full - * or until the end of the file is reached. Because this method - * always receives complete command-lines, they can go directly - * into the main command queue. + * 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 GCodeQueue::get_sdcard_commands() { - static uint8_t sd_input_state = PS_NORMAL; + static bool stop_buffering = false, + sd_comment_mode = false + #if ENABLED(PAREN_COMMENTS) + , sd_comment_paren_mode = false + #endif + ; if (!IS_SD_PRINTING()) return; - int sd_count = 0; + /** + * '#' stops reading from SD to the buffer prematurely, so procedural + * macro calls are possible. If it occurs, stop_buffering is triggered + * and the buffer is run dry; this character _can_ occur in serial com + * due to checksums, however, no checksums are used in SD printing. + */ + + if (length == 0) stop_buffering = false; + + uint16_t sd_count = 0; bool card_eof = card.eof(); - while (length < BUFSIZE && !card_eof) { + while (length < BUFSIZE && !card_eof && !stop_buffering) { const int16_t n = card.get(); + char sd_char = (char)n; card_eof = card.eof(); - if (n < 0 && !card_eof) { SERIAL_ERROR_MSG(STR_SD_ERR_READ); continue; } - - const char sd_char = (char)n; - const bool is_eol = ISEOL(sd_char); - if (is_eol || card_eof) { - - // Reset stream state, terminate the buffer, and commit a non-empty command - if (!is_eol && sd_count) ++sd_count; // End of file with no newline - if (!process_line_done(sd_input_state, command_buffer[index_w], sd_count)) { - _commit_command(false); - #if ENABLED(POWER_LOSS_RECOVERY) - recovery.cmd_sdpos = card.getIndex(); // Prime for the NEXT _commit_command - #endif + if (card_eof || n == -1 + || sd_char == '\n' || sd_char == '\r' + || ((sd_char == '#' || sd_char == ':') && !sd_comment_mode + #if ENABLED(PAREN_COMMENTS) + && !sd_comment_paren_mode + #endif + ) + ) { + if (card_eof) { + + card.printingHasFinished(); + + if (IS_SD_PRINTING()) + sd_count = 0; // If a sub-file was printing, continue from call point + else { + SERIAL_ECHOLNPGM(MSG_FILE_PRINTED); + #if ENABLED(PRINTER_EVENT_LEDS) + printerEventLEDs.onPrintCompleted(); + #if HAS_RESUME_CONTINUE + enqueue_now_P(PSTR("M0 Q S" + #if HAS_LCD_MENU + "1800" + #else + "60" + #endif + )); + #endif + #endif + } } + else if (n == -1) + SERIAL_ERROR_MSG(MSG_SD_ERR_READ); - if (card_eof) card.fileHasFinished(); // Handle end of file reached - } - else - process_stream_char(sd_char, sd_input_state, command_buffer[index_w], sd_count); + if (sd_char == '#') stop_buffering = true; + + sd_comment_mode = false; // for new command + #if ENABLED(PAREN_COMMENTS) + sd_comment_paren_mode = false; + #endif + + // Skip empty lines and comments + if (!sd_count) { thermalManager.manage_heater(); continue; } + + command_buffer[index_w][sd_count] = '\0'; // terminate string + sd_count = 0; // clear sd line buffer + _commit_command(false); + + #if ENABLED(POWER_LOSS_RECOVERY) + recovery.cmd_sdpos = card.getIndex(); // Prime for the next _commit_command + #endif + } + else if (sd_count >= MAX_CMD_SIZE - 1) { + /** + * Keep fetching, but ignore normal characters beyond the max length + * The command will be injected when EOL is reached + */ + } + else { + if (sd_char == ';') sd_comment_mode = true; + #if ENABLED(PAREN_COMMENTS) + else if (sd_char == '(') sd_comment_paren_mode = true; + else if (sd_char == ')') sd_comment_paren_mode = false; + #endif + else if (!sd_comment_mode + #if ENABLED(PAREN_COMMENTS) + && ! sd_comment_paren_mode + #endif + ) command_buffer[index_w][sd_count++] = sd_char; + } } } @@ -593,7 +592,7 @@ void GCodeQueue::get_serial_commands() { /** * Add to the circular command queue the next command from: - * - The command-injection queues (injected_commands_P, injected_commands) + * - The command-injection queue (injected_commands_P) * - The active serial input (usually USB) * - The SD card file being actively printed */ @@ -601,7 +600,9 @@ void GCodeQueue::get_available_commands() { get_serial_commands(); - TERN_(SDSUPPORT, get_sdcard_commands()); + #if ENABLED(SDSUPPORT) + get_sdcard_commands(); + #endif } /** @@ -610,7 +611,7 @@ void GCodeQueue::get_available_commands() { void GCodeQueue::advance() { // Process immediate commands - if (process_injected_command_P() || process_injected_command()) return; + if (process_injected_command()) return; // Return if the G-code buffer is empty if (!length) return; @@ -622,7 +623,7 @@ void GCodeQueue::advance() { if (is_M29(command)) { // M29 closes the file card.closefile(); - SERIAL_ECHOLNPGM(STR_FILE_SAVED); + SERIAL_ECHOLNPGM(MSG_FILE_SAVED); #if !defined(__AVR__) || !defined(USBCON) #if ENABLED(SERIAL_STATS_DROPPED_RX) @@ -655,7 +656,9 @@ void GCodeQueue::advance() { #endif // SDSUPPORT // The queue may be reset by a command handler or by code invoked by idle() within a handler - --length; - if (++index_r >= BUFSIZE) index_r = 0; + if (length) { + --length; + if (++index_r >= BUFSIZE) index_r = 0; + } }