From 166d4a26f34d45a211e947d279e9f23c6d2f3c96 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <thinkyhead@users.noreply.github.com>
Date: Fri, 12 Mar 2021 08:34:22 -0600
Subject: [PATCH] Pause SD queue early on M25 (#21317)

---
 Marlin/src/gcode/host/M115.cpp  | 3 +++
 Marlin/src/gcode/queue.cpp      | 8 +++++++-
 Marlin/src/gcode/sd/M24_M25.cpp | 4 ++++
 3 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/Marlin/src/gcode/host/M115.cpp b/Marlin/src/gcode/host/M115.cpp
index cd6fcbaf8..a518e68cf 100644
--- a/Marlin/src/gcode/host/M115.cpp
+++ b/Marlin/src/gcode/host/M115.cpp
@@ -120,6 +120,9 @@ void GcodeSuite::M115() {
     // REPEAT (M808)
     cap_line(PSTR("REPEAT"), ENABLED(GCODE_REPEAT_MARKERS));
 
+    // SD_WRITE (M928, M28, M29)
+    cap_line(PSTR("SD_WRITE"), ENABLED(SDSUPPORT) && DISABLED(SDCARD_READONLY));
+
     // AUTOREPORT_SD_STATUS (M27 extension)
     cap_line(PSTR("AUTOREPORT_SD_STATUS"), ENABLED(AUTO_REPORT_SD_STATUS));
 
diff --git a/Marlin/src/gcode/queue.cpp b/Marlin/src/gcode/queue.cpp
index b093184c9..7845b01ad 100644
--- a/Marlin/src/gcode/queue.cpp
+++ b/Marlin/src/gcode/queue.cpp
@@ -567,7 +567,6 @@ void GCodeQueue::get_serial_commands() {
       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, sd_count)) {
@@ -575,6 +574,13 @@ void GCodeQueue::get_serial_commands() {
           // M808 L saves the sdpos of the next line. M808 loops to a new sdpos.
           TERN_(GCODE_REPEAT_MARKERS, repeat.early_parse_M808(command.buffer));
 
+          #if DISABLED(PARK_HEAD_ON_PAUSE)
+            // When M25 is non-blocking it can still suspend SD commands
+            // Otherwise the M125 handler needs to know SD printing is active
+            if (command.buffer[0] == 'M' && command.buffer[1] == '2' && command.buffer[2] == '5' && !NUMERIC(command.buffer[3]))
+              card.pauseSDPrint();
+          #endif
+
           // Put the new command into the buffer (no "ok" sent)
           ring_buffer.commit_command(true);
 
diff --git a/Marlin/src/gcode/sd/M24_M25.cpp b/Marlin/src/gcode/sd/M24_M25.cpp
index 8fb529dca..1c98791bc 100644
--- a/Marlin/src/gcode/sd/M24_M25.cpp
+++ b/Marlin/src/gcode/sd/M24_M25.cpp
@@ -87,6 +87,10 @@ void GcodeSuite::M24() {
 
 /**
  * M25: Pause SD Print
+ *
+ * With PARK_HEAD_ON_PAUSE:
+ *   Invoke M125 to store the current position and move to the park
+ *   position. M24 will move the head back before resuming the print.
  */
 void GcodeSuite::M25() {