From b586b6411967ffcfd3f3dedc0097aee6bc81ee8c Mon Sep 17 00:00:00 2001
From: fedetony <45215920+fedetony@users.noreply.github.com>
Date: Sat, 17 Apr 2021 10:54:43 +0200
Subject: [PATCH] Fix Realtime Reporting with G-code Motion Modes (#21630)

---
 Marlin/src/gcode/bedlevel/abl/G29.cpp |  1 +
 Marlin/src/gcode/parser.cpp           | 18 +++++++++++++-----
 2 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/Marlin/src/gcode/bedlevel/abl/G29.cpp b/Marlin/src/gcode/bedlevel/abl/G29.cpp
index 8cc0a66216..735fad015e 100644
--- a/Marlin/src/gcode/bedlevel/abl/G29.cpp
+++ b/Marlin/src/gcode/bedlevel/abl/G29.cpp
@@ -901,6 +901,7 @@ G29_TYPE GcodeSuite::G29() {
   TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE));
 
   G29_RETURN(isnan(abl.measured_z));
+
 }
 
 #endif // HAS_ABL_NOT_UBL
diff --git a/Marlin/src/gcode/parser.cpp b/Marlin/src/gcode/parser.cpp
index 9f0d09563e..8cbbe1d19d 100644
--- a/Marlin/src/gcode/parser.cpp
+++ b/Marlin/src/gcode/parser.cpp
@@ -217,10 +217,10 @@ void GCodeParser::parse(char *p) {
 
     #if ENABLED(GCODE_MOTION_MODES)
       #if ENABLED(ARC_SUPPORT)
-        case 'I' ... 'J': case 'R':
+        case 'I' ... 'J': 
           if (motion_mode_codenum != 2 && motion_mode_codenum != 3) return;
       #endif
-      case 'P' ... 'Q':
+      case 'Q':
         if (motion_mode_codenum != 5) return;
       case 'X' ... 'Z': case 'E' ... 'F':
         if (motion_mode_codenum < 0) return;
@@ -229,15 +229,23 @@ void GCodeParser::parse(char *p) {
         TERN_(USE_GCODE_SUBCODES, subcode = motion_mode_subcode);
         p--; // Back up one character to use the current parameter
       break;
-    #endif // GCODE_MOTION_MODES
+    #endif
 
     #if ENABLED(REALTIME_REPORTING_COMMANDS)
-      case 'S': case 'P': case 'R': {
+      case 'P': case 'R': {
+        if (letter == 'R') {
+          #if ENABLED(GCODE_MOTION_MODES)
+            if (ENABLED(ARC_SUPPORT) && !WITHIN(motion_mode_codenum, 2, 3)) return;
+          #endif
+        }
+        else if (TERN0(GCODE_MOTION_MODES, motion_mode_codenum != 5)) return;
+      } // fall-thru
+      case 'S': {
         codenum = 0;                  // The only valid codenum is 0
         uint8_t digits = 0;
         while (*p++ == '0') digits++; // Count up '0' characters
         command_letter = (digits == 3) ? letter : '?'; // Three '0' digits is a good command
-      } return;                       // No parameters, so return
+      } return;                       // No parameters needed, so return now
     #endif
 
     default: return;