MMU2 S Mode spins the BMG gears during C0 (#19429)

Marlin/src/feature/mmu2/mmu2.cpp

 #define MMU_CMD_TIMEOUT 45000UL // 45s timeout for mmu commands (except P0)
 #define MMU_P0_TIMEOUT 3000UL   // Timeout for P0 command: 3seconds
 
+#define MMU2_COMMAND(S) tx_str_P(PSTR(S "\n"))
+
 #if ENABLED(MMU_EXTRUDER_SENSOR)
   uint8_t mmu_idl_sens = 0;
   static bool mmu_loading_flag = false;
     safe_delay(20);
     WRITE(MMU2_RST_PIN, HIGH);
   #else
-    tx_str_P(PSTR("X0\n")); // Send soft reset
+    MMU2_COMMAND("X0"); // Send soft reset
   #endif
 }
 
         DEBUG_ECHOLNPGM("MMU => 'start'");
         DEBUG_ECHOLNPGM("MMU <= 'S1'");
 
-        // send "read version" request
-        tx_str_P(PSTR("S1\n"));
-
+        MMU2_COMMAND("S1");   // Read Version
         state = -2;
       }
       else if (millis() > 3000000) {
 
         DEBUG_ECHOLNPAIR("MMU => ", version, "\nMMU <= 'S2'");
 
-        tx_str_P(PSTR("S2\n")); // read build number
+        MMU2_COMMAND("S2");   // Read Build Number
         state = -3;
       }
       break;
         #if ENABLED(MMU2_MODE_12V)
           DEBUG_ECHOLNPGM("MMU <= 'M1'");
 
-          tx_str_P(PSTR("M1\n")); // switch to stealth mode
+          MMU2_COMMAND("M1");   // Stealth Mode
           state = -5;
 
         #else
           DEBUG_ECHOLNPGM("MMU <= 'P0'");
 
-          tx_str_P(PSTR("P0\n")); // read finda
+          MMU2_COMMAND("P0");   // Read FINDA
           state = -4;
         #endif
       }
 
         DEBUG_ECHOLNPGM("MMU <= 'P0'");
 
-        tx_str_P(PSTR("P0\n")); // read finda
+        MMU2_COMMAND("P0");   // Read FINDA
         state = -4;
       }
       break;
         else if (cmd == MMU_CMD_C0) {
           // continue loading
           DEBUG_ECHOLNPGM("MMU <= 'C0'");
-          tx_str_P(PSTR("C0\n"));
+          MMU2_COMMAND("C0");
           state = 3; // wait for response
         }
         else if (cmd == MMU_CMD_U0) {
           // unload current
           DEBUG_ECHOLNPGM("MMU <= 'U0'");
 
-          tx_str_P(PSTR("U0\n"));
+          MMU2_COMMAND("U0");
           state = 3; // wait for response
         }
         else if (WITHIN(cmd, MMU_CMD_E0, MMU_CMD_E4)) {
         else if (cmd == MMU_CMD_R0) {
           // recover after eject
           DEBUG_ECHOLNPGM("MMU <= 'R0'");
-          tx_str_P(PSTR("R0\n"));
+          MMU2_COMMAND("R0");
           state = 3; // wait for response
         }
         else if (WITHIN(cmd, MMU_CMD_F0, MMU_CMD_F4)) {
         cmd = MMU_CMD_NONE;
       }
       else if (ELAPSED(millis(), prev_P0_request + 300)) {
-        // read FINDA
-        tx_str_P(PSTR("P0\n"));
+        MMU2_COMMAND("P0"); // Read FINDA
         state = 2; // wait for response
       }
 
       #if ENABLED(MMU_EXTRUDER_SENSOR)
         if (mmu_idl_sens) {
           if (FILAMENT_PRESENT() && mmu_loading_flag) {
-            DEBUG_ECHOLNPGM("MMU <= 'A'\n");
-            tx_str_P(PSTR("A\n")); // send 'abort' request
+            DEBUG_ECHOLNPGM("MMU <= 'A'");
+            MMU2_COMMAND("A"); // send 'abort' request
             mmu_idl_sens = 0;
-            DEBUG_ECHOLNPGM("MMU IDLER_SENSOR = 0 - ABORT\n");
+            DEBUG_ECHOLNPGM("MMU IDLER_SENSOR = 0 - ABORT");
           }
         }
       #endif
 
       if (rx_ok()) {
-        DEBUG_ECHOLNPGM("MMU => 'ok'");
-        ready = true;
-        state = 1;
-        last_cmd = MMU_CMD_NONE;
+        // response to C0 mmu command in PRUSA_MMU2_S_MODE
+        bool can_reset = true;
+        if (ENABLED(PRUSA_MMU2_S_MODE) && last_cmd == MMU_CMD_C0) {
+          if (!mmu2s_triggered) {
+            can_reset = false;
+            // MMU ok received but filament sensor not triggered, retrying...
+            DEBUG_ECHOLNPGM("MMU => 'ok' (filament not present in gears)");
+            DEBUG_ECHOLNPGM("MMU <= 'C0' (keep trying)");
+            MMU2_COMMAND("C0");
+          }
+        }
+        if (can_reset) {
+          DEBUG_ECHOLNPGM("MMU => 'ok'");
+          ready = true;
+          state = 1;
+          last_cmd = MMU_CMD_NONE;
+        }
       }
       else if (ELAPSED(millis(), prev_request + MMU_CMD_TIMEOUT)) {
         // resend request after timeout
 
   void MMU2::check_filament() {
     const bool present = FILAMENT_PRESENT();
-    if (present && !mmu2s_triggered) {
-      DEBUG_ECHOLNPGM("MMU <= 'A'");
-      tx_str_P(PSTR("A\n"));
+    if (cmd == MMU_CMD_NONE && last_cmd == MMU_CMD_C0) {
+      if (present && !mmu2s_triggered) {
+        DEBUG_ECHOLNPGM("MMU <= 'A'");
+        tx_str_P(PSTR("A\n"));
+      }
+      // Slowly spin the extruder during C0
+      else {
+        while (planner.movesplanned() < 3) {
+          current_position.e += 0.25;
+          line_to_current_position(MMM_TO_MMS(120));
+        }
+      }
     }
     mmu2s_triggered = present;
   }