MMU: Allow reading/writing registers while handling an error

Fixes prusa3d#4558

Change in memory:
Flash: +136 bytes
SRAM: +1 byte

Firmware/mmu2.cpp

@@ -159,9 +159,11 @@ bool MMU2::ReadRegister(uint8_t address) {
     if (!WaitForMMUReady()) {
         return false;
     }
+
+    RequestMsg msg = RequestMsg(RequestMsgCodes::Read, address);
     do {
         logic.ReadRegister(address); // we may signal the accepted/rejected status of the response as return value of this function
-    } while (!manage_response(false, false));
+    } while (!manage_response(false, false, &msg));
 
     // Update cached value
     lastReadRegisterValue = logic.rsp.paramValue;
@@ -185,9 +187,10 @@ bool __attribute__((noinline)) MMU2::WriteRegister(uint8_t address, uint16_t dat
         break; // do not intercept any other register writes
     }
 
+    RequestMsg msg = RequestMsg(RequestMsgCodes::Write, address);
     do {
         logic.WriteRegister(address, data); // we may signal the accepted/rejected status of the response as return value of this function
-    } while (!manage_response(false, false));
+    } while (!manage_response(false, false, &msg));
 
     return true;
 }
@@ -786,14 +789,36 @@ void MMU2::CheckUserInput() {
     }
 }
 
+bool MMU2::check_nested_response(RequestMsg * msg) {
+    // If we see a response that matches RequestMsg, then return
+    if (!msg) {
+        return false;
+    }
+    return logic.rsp.request.code == msg->code
+        && logic.rsp.request.value == msg->value
+        && logic.rsp.paramCode == ResponseMsgParamCodes::Accepted;
+}
+
+
 /// Originally, this was used to wait for response and deal with timeout if necessary.
 /// The new protocol implementation enables much nicer and intense reporting, so this method will boil down
 /// just to verify the result of an issued command (which was basically the original idea)
 ///
 /// It is closely related to mmu_loop() (which corresponds to our ProtocolLogic::Step()), which does NOT perform any blocking wait for a command to finish.
 /// But - in case of an error, the command is not yet finished, but we must react accordingly - move the printhead elsewhere, stop heating, eat a cat or so.
 /// That's what's being done here...
-bool MMU2::manage_response(const bool move_axes, const bool turn_off_nozzle) {
+bool MMU2::manage_response(const bool move_axes, const bool turn_off_nozzle, RequestMsg *msg) {
+    static uint8_t responseDepth = 0;
+    struct ResponseRAII {
+        ResponseRAII() {responseDepth++;}
+        ~ResponseRAII() {responseDepth--;}
+    } responseRAII;
+
+    // The printer may read and write registers in order to recover from MMU errors
+    if (responseDepth == 2) {
+        return check_nested_response(msg);
+    }
+
     mmu_print_saved = SavedState::None;
 
     MARLIN_KEEPALIVE_STATE_IN_PROCESS;

Firmware/mmu2.h

@@ -261,7 +261,12 @@ class MMU2 {
     /// Along with the mmu_loop method, this loops until a response from the MMU is received and acts upon.
     /// In case of an error, it parks the print head and turns off nozzle heating
     /// @returns false if the command could not have been completed (MMU interrupted)
-    [[nodiscard]] bool manage_response(const bool move_axes, const bool turn_off_nozzle);
+    [[nodiscard]] bool manage_response(const bool move_axes, const bool turn_off_nozzle, RequestMsg * msg = nullptr);
+
+    /// Check whether a nested request has a received a response
+    /// @param msg Message which the Response must match
+    /// @return true if the response matches the request
+    bool check_nested_response(RequestMsg * msg);
 
     /// The inner private implementation of mmu_loop()
     /// which is NOT (!!!) recursion-guarded. Use caution - but we do need it during waiting for hotend resume to keep comms alive!