Optimize MMU protocol logic

Firmware/mmu2.h

@@ -173,7 +173,7 @@ public:
     /// In the future we'll return the trully detected FW version
     Version GetMMUFWVersion()const {
         if( State() == xState::Active ){
-            return { logic.MmuFwVersionMajor(), logic.MmuFwVersionMinor(), logic.MmuFwVersionBuild() };
+            return { logic.MmuFwVersionMajor(), logic.MmuFwVersionMinor(), logic.MmuFwVersionRevision() };
         } else {
             return { 0, 0, 0}; 
         }

Firmware/mmu2_protocol_logic.cpp

@@ -6,35 +6,33 @@
 
 namespace MMU2 {
 
-static constexpr uint8_t supportedMmuFWVersionMajor = 2;
-static constexpr uint8_t supportedMmuFWVersionMinor = 1;
-static constexpr uint8_t supportedMmuFWVersionBuild = 1;
+static const uint8_t supportedMmuFWVersion[3] PROGMEM = { 2, 1, 1 };
 
-void ProtocolLogic::CheckAndReportAsyncEvents(){
+void ProtocolLogic::CheckAndReportAsyncEvents() {
     // even when waiting for a query period, we need to report a change in filament sensor's state
     // - it is vital for a precise synchronization of moves of the printer and the MMU
     uint8_t fs = (uint8_t)WhereIsFilament();
-    if( fs != lastFSensor ){
+    if (fs != lastFSensor) {
         SendAndUpdateFilamentSensor();
     }
 }
 
-void ProtocolLogic::SendQuery(){
+void ProtocolLogic::SendQuery() {
     SendMsg(RequestMsg(RequestMsgCodes::Query, 0));
     scopeState = ScopeState::QuerySent;
 }
 
-void ProtocolLogic::SendFINDAQuery(){
-    SendMsg(RequestMsg(RequestMsgCodes::Finda, 0 ) );
+void ProtocolLogic::SendFINDAQuery() {
+    SendMsg(RequestMsg(RequestMsgCodes::Finda, 0));
     scopeState = ScopeState::FINDAReqSent;
 }
 
-void ProtocolLogic::SendAndUpdateFilamentSensor(){
-    SendMsg(RequestMsg(RequestMsgCodes::FilamentSensor, lastFSensor = (uint8_t)WhereIsFilament() ) );
+void ProtocolLogic::SendAndUpdateFilamentSensor() {
+    SendMsg(RequestMsg(RequestMsgCodes::FilamentSensor, lastFSensor = (uint8_t)WhereIsFilament()));
     scopeState = ScopeState::FilamentSensorStateSent;
 }
 
-void ProtocolLogic::SendButton(uint8_t btn){
+void ProtocolLogic::SendButton(uint8_t btn) {
     SendMsg(RequestMsg(RequestMsgCodes::Button, btn));
     scopeState = ScopeState::ButtonSent;
 }
@@ -73,14 +71,14 @@ struct OldMMUFWDetector {
     }
 };
 
-StepStatus ProtocolLogic::ExpectingMessage(uint32_t timeout) {
+StepStatus ProtocolLogic::ExpectingMessage() {
     int bytesConsumed = 0;
     int c = -1;
     
     OldMMUFWDetector oldMMUh4x0r; // old MMU FW hacker ;)
         
     // try to consume as many rx bytes as possible (until a message has been completed)
-    while((c = uart->read()) >= 0){
+    while ((c = uart->read()) >= 0) {
         ++bytesConsumed;
         RecordReceivedByte(c);
         switch (protocol.DecodeResponse(c)) {
@@ -109,10 +107,10 @@ StepStatus ProtocolLogic::ExpectingMessage(uint32_t timeout) {
             return ProtocolError;
         }
     }
-    if( bytesConsumed != 0 ){
+    if (bytesConsumed != 0) {
         RecordUARTActivity(); // something has happened on the UART, update the timeout record
-        return Processing; // consumed some bytes, but message still not ready
-    } else if (Elapsed(timeout)) {
+        return Processing;    // consumed some bytes, but message still not ready
+    } else if (Elapsed(linkLayerTimeout)) {
         return CommunicationTimeout;
     }
     return Processing;
@@ -144,123 +142,147 @@ void ProtocolLogic::IdleRestart() {
     scopeState = ScopeState::Ready;
 }
 
-StepStatus ProtocolLogic::StartSeqStep(){
-    if (auto expmsg = ExpectingMessage(linkLayerTimeout); expmsg != MessageReady)
-        return expmsg;
-    
-    // solve initial handshake
-    switch (scopeState) {
-    case ScopeState::S0Sent: // received response to S0 - major
-        if( rsp.request.code != RequestMsgCodes::Version || rsp.request.value != 0 ){
-            // got a response to something else - protocol corruption probably, repeat the query
-            SendVersion(0);
-        } else {
-            mmuFwVersionMajor = rsp.paramValue;
-            if (mmuFwVersionMajor != supportedMmuFWVersionMajor) {
-                if( --retries == 0){
-                    // if (--retries == 0) has a specific meaning - since we are losing bytes on the UART for no obvious reason
-                    // it can happen, that the reported version number is not complete - i.e. "1" instead of "19"
-                    // Therefore we drop the MMU only if we run out of retries for this very reason.
-                    // There is a limited amount of retries per the whole start seq.
-                    // We also must be able to actually detect an unsupported MMU FW version, so the amount of retries shall be kept small.
-                    return VersionMismatch;
-                } else {
-                    SendVersion(0);
-                }
+StepStatus ProtocolLogic::ProcessVersionResponse(uint8_t stage) {
+    if (rsp.request.code != RequestMsgCodes::Version || rsp.request.value != stage) {
+        // got a response to something else - protocol corruption probably, repeat the query OR restart the comm by issuing S0?
+        SendVersion(stage);
+    } else {
+        mmuFwVersion[stage] = rsp.paramValue;
+        if (mmuFwVersion[stage] != pgm_read_byte(supportedMmuFWVersion[stage])) {
+            if (--retries == 0) {
+                return VersionMismatch;
             } else {
-                dataTO.Reset(); // got meaningful response from the MMU, stop data layer timeout tracking
-                SendVersion(1);
+                SendVersion(stage);
             }
-        }
-        break;
-    case ScopeState::S1Sent: // received response to S1 - minor
-        if( rsp.request.code != RequestMsgCodes::Version || rsp.request.value != 1 ){
-            // got a response to something else - protocol corruption probably, repeat the query OR restart the comm by issuing S0?
-            SendVersion(1);
         } else {
-            mmuFwVersionMinor = rsp.paramValue;
-            if (mmuFwVersionMinor != supportedMmuFWVersionMinor){
-                if( --retries == 0) {
-                    return VersionMismatch;
-                } else {
-                    SendVersion(1);
-                }
-            } else {
-                SendVersion(2);
-            }
+            dataTO.Reset(); // got a meaningful response from the MMU, stop data layer timeout tracking
+            SendVersion(stage + 1);
         }
-        break;
-    case ScopeState::S2Sent: // received response to S2 - revision
-        if( rsp.request.code != RequestMsgCodes::Version || rsp.request.value != 2 ){
+    }
+    return Processing;
+}
+
+StepStatus ProtocolLogic::ScopeStep() {
+    if ((uint_fast8_t)scopeState & (uint8_t)ScopeState::NotExpectsResponse) {
+        // we are waiting for something
+        switch (currentScope) {
+        case Scope::DelayedRestart:
+            return DelayedRestartWait();
+        case Scope::Idle:
+            return IdleWait();
+        case Scope::Command:
+            return CommandWait();
+        case Scope::Stopped:
+            return StoppedStep();
+        default:
+            break;
+        }
+    } else {
+        // we are expecting a message
+        if (auto expmsg = ExpectingMessage(); expmsg != MessageReady) // this whole statement takes 12B
+            return expmsg;
+
+        // process message
+        switch (currentScope) {
+        case Scope::StartSeq:
+            return StartSeqStep(); // ~270B
+        case Scope::Idle:
+            return IdleStep(); // ~300B
+        case Scope::Command:
+            return CommandStep(); // ~430B
+        case Scope::Stopped:
+            return StoppedStep();
+        default:
+            break;
+        }
+    }
+    return Finished;
+}
+
+StepStatus ProtocolLogic::StartSeqStep() {
+    // solve initial handshake
+    switch (scopeState) {
+    case ScopeState::S0Sent: // received response to S0 - major
+    case ScopeState::S1Sent: // received response to S1 - minor
+    case ScopeState::S2Sent: // received response to S2 - minor
+        return ProcessVersionResponse((uint8_t)scopeState - (uint8_t)ScopeState::S0Sent);
+    case ScopeState::S3Sent: // received response to S3 - revision
+        if (rsp.request.code != RequestMsgCodes::Version || rsp.request.value != 3) {
             // got a response to something else - protocol corruption probably, repeat the query OR restart the comm by issuing S0?
-            SendVersion(2);
+            SendVersion(3);
         } else {
-            mmuFwVersionBuild = rsp.paramValue;
-            if (mmuFwVersionBuild < supportedMmuFWVersionBuild){
-                if( --retries == 0 ) {
-                    return VersionMismatch;
-                } else {
-                    SendVersion(2);
-                }
-            } else {
-                // Start General Interrogation after line up.
-                // For now we just send the state of the filament sensor, but we may request
-                // data point states from the MMU as well. TBD in the future, especially with another protocol
-                SendAndUpdateFilamentSensor();
-            }
+            mmuFwVersionBuild = rsp.paramValue; // just register the build number
+            // Start General Interrogation after line up.
+            // For now we just send the state of the filament sensor, but we may request
+            // data point states from the MMU as well. TBD in the future, especially with another protocol
+            SendAndUpdateFilamentSensor();
         }
-        break;
+        return Processing;
     case ScopeState::FilamentSensorStateSent:
-        scopeState = ScopeState::Ready;
         SwitchFromStartToIdle();
         return Processing; // Returning Finished is not a good idea in case of a fast error recovery
         // - it tells the printer, that the command which experienced a protocol error and recovered successfully actually terminated.
         // In such a case we must return "Processing" in order to keep the MMU state machine running and prevent the printer from executing next G-codes.
         break;
-    case ScopeState::RecoveringProtocolError:
-        // timer elapsed, clear the input buffer
-        while (uart->read() >= 0)
-            ;
-        SendVersion(0);
-        break;
     default:
         return VersionMismatch;
     }
     return Finished;
 }
 
-StepStatus ProtocolLogic::DelayedRestartStep() {
-    switch (scopeState) {
-    case ScopeState::RecoveringProtocolError:
-        if (Elapsed(heartBeatPeriod)) { // this basically means, that we are waiting until there is some traffic on
-            while (uart->read() != -1)
-                ; // clear the input buffer
-            // switch to StartSeq
-            Start();
-        }
+StepStatus ProtocolLogic::DelayedRestartWait() {
+    if (Elapsed(heartBeatPeriod)) { // this basically means, that we are waiting until there is some traffic on
+        while (uart->read() != -1)
+            ; // clear the input buffer
+        // switch to StartSeq
+        Start();
+    }
+    return Processing;
+}
+
+StepStatus ProtocolLogic::CommandWait() {
+    if (Elapsed(heartBeatPeriod)) {
+        SendQuery();
+    } else {
+        // even when waiting for a query period, we need to report a change in filament sensor's state
+        // - it is vital for a precise synchronization of moves of the printer and the MMU
+        CheckAndReportAsyncEvents();
+    }
+    return Processing;
+}
+
+StepStatus ProtocolLogic::ProcessCommandQueryResponse() {
+    switch (rsp.paramCode) {
+    case ResponseMsgParamCodes::Processing:
+        progressCode = static_cast<ProgressCode>(rsp.paramValue);
+        errorCode = ErrorCode::OK;
+        SendAndUpdateFilamentSensor(); // keep on reporting the state of fsensor regularly
         return Processing;
-        break;
+    case ResponseMsgParamCodes::Error:
+        // in case of an error the progress code remains as it has been before
+        errorCode = static_cast<ErrorCode>(rsp.paramValue);
+        // keep on reporting the state of fsensor regularly even in command error state
+        // - the MMU checks FINDA and fsensor even while recovering from errors
+        SendAndUpdateFilamentSensor();
+        return CommandError;
+    case ResponseMsgParamCodes::Button:
+        // The user pushed a button on the MMU. Save it, do what we need to do
+        // to prepare, then pass it back to the MMU so it can work its magic.
+        buttonCode = static_cast<Buttons>(rsp.paramValue);
+        SendAndUpdateFilamentSensor();
+        return ButtonPushed;
+    case ResponseMsgParamCodes::Finished:
+        progressCode = ProgressCode::OK;
+        scopeState = ScopeState::Ready;
+        return Finished;
     default:
-        break;
+        return ProtocolError;
     }
-    return Finished;
 }
 
 StepStatus ProtocolLogic::CommandStep() {
     switch (scopeState) {
-    case ScopeState::Wait:
-        if (Elapsed(heartBeatPeriod)) {
-            SendQuery();
-        } else { 
-            // even when waiting for a query period, we need to report a change in filament sensor's state
-            // - it is vital for a precise synchronization of moves of the printer and the MMU
-            CheckAndReportAsyncEvents();
-        }
-        break;
     case ScopeState::CommandSent: {
-        if (auto expmsg = ExpectingMessage(linkLayerTimeout); expmsg != MessageReady)
-            return expmsg;
-
         switch (rsp.paramCode) { // the response should be either accepted or rejected
         case ResponseMsgParamCodes::Accepted:
             progressCode = ProgressCode::OK;
@@ -275,59 +297,21 @@ StepStatus ProtocolLogic::CommandStep() {
         default:
             return ProtocolError;
         }
-        } break;
+    } break;
     case ScopeState::QuerySent:
-        if (auto expmsg = ExpectingMessage(linkLayerTimeout); expmsg != MessageReady)
-            return expmsg;
-        [[fallthrough]];
-    case ScopeState::ContinueFromIdle:
-        switch (rsp.paramCode) {
-        case ResponseMsgParamCodes::Processing:
-            progressCode = static_cast<ProgressCode>(rsp.paramValue);
-            errorCode = ErrorCode::OK;
-            SendAndUpdateFilamentSensor(); // keep on reporting the state of fsensor regularly
-            break;
-        case ResponseMsgParamCodes::Error:
-            // in case of an error the progress code remains as it has been before
-            errorCode = static_cast<ErrorCode>(rsp.paramValue);
-            // keep on reporting the state of fsensor regularly even in command error state
-            // - the MMU checks FINDA and fsensor even while recovering from errors
-            SendAndUpdateFilamentSensor();
-            return CommandError;
-        case ResponseMsgParamCodes::Button:
-            // The user pushed a button on the MMU. Save it, do what we need to do 
-            // to prepare, then pass it back to the MMU so it can work its magic.
-            buttonCode = static_cast<Buttons>(rsp.paramValue);
-            SendAndUpdateFilamentSensor();
-            return ButtonPushed;
-        case ResponseMsgParamCodes::Finished:
-            progressCode = ProgressCode::OK;
-            scopeState = ScopeState::Ready;
-            return Finished;
-        default:
-            return ProtocolError;
-        }
-        break;
+        return ProcessCommandQueryResponse();
     case ScopeState::FilamentSensorStateSent:
-        if (auto expmsg = ExpectingMessage(linkLayerTimeout); expmsg != MessageReady)
-            return expmsg;
         SendFINDAQuery();
         scopeState = ScopeState::FINDAReqSent;
         return Processing;
     case ScopeState::FINDAReqSent:
-        if (auto expmsg = ExpectingMessage(linkLayerTimeout); expmsg != MessageReady)
-            return expmsg;
-        SendReadRegister(3, ScopeState::StatisticsSent);
+        SendReadRegister(4, ScopeState::StatisticsSent);
         scopeState = ScopeState::StatisticsSent;
         return Processing;
     case ScopeState::StatisticsSent:
-        if (auto expmsg = ExpectingMessage(linkLayerTimeout); expmsg != MessageReady)
-            return expmsg;
         scopeState = ScopeState::Wait;
         return Processing;
     case ScopeState::ButtonSent:
-        if (auto expmsg = ExpectingMessage(linkLayerTimeout); expmsg != MessageReady)
-            return expmsg;
         if (rsp.paramCode == ResponseMsgParamCodes::Accepted) {
             // Button was accepted, decrement the retry.
             mmu2.DecrementRetryAttempts();
@@ -340,83 +324,82 @@ StepStatus ProtocolLogic::CommandStep() {
     return Processing;
 }
 
-StepStatus ProtocolLogic::IdleStep() {
-    if(scopeState == ScopeState::Ready){ // check timeout
+StepStatus ProtocolLogic::IdleWait() {
+    if (scopeState == ScopeState::Ready) { // check timeout
         if (Elapsed(heartBeatPeriod)) {
             SendQuery();
             return Processing;
         }
-    } else {
-        if (auto expmsg = ExpectingMessage(linkLayerTimeout); expmsg != MessageReady)
-            return expmsg;
-        
-        switch (scopeState) {
-        case ScopeState::QuerySent: // check UART
-            // If we are accidentally in Idle and we receive something like "T0 P1" - that means the communication dropped out while a command was in progress.
-            // That causes no issues here, we just need to switch to Command processing and continue there from now on.
-            // The usual response in this case should be some command and "F" - finished - that confirms we are in an Idle state even on the MMU side.
-            switch( rsp.request.code ){
-            case RequestMsgCodes::Cut:
-            case RequestMsgCodes::Eject:
-            case RequestMsgCodes::Load:
-            case RequestMsgCodes::Mode:
-            case RequestMsgCodes::Tool:
-            case RequestMsgCodes::Unload:
-                if( rsp.paramCode != ResponseMsgParamCodes::Finished ){
-                    SwitchFromIdleToCommand();
-                    return Processing;
-                }
-                break;
-            case RequestMsgCodes::Reset:
-                // this one is kind of special
-                // we do not transfer to any "running" command (i.e. we stay in Idle),
-                // but in case there is an error reported we must make sure it gets propagated
-                switch( rsp.paramCode ){
-                case ResponseMsgParamCodes::Button:
-                    // The user pushed a button on the MMU. Save it, do what we need to do 
-                    // to prepare, then pass it back to the MMU so it can work its magic.
-                    buttonCode = static_cast<Buttons>(rsp.paramValue);
-                    SendFINDAQuery();
-                    return ButtonPushed;
-                case ResponseMsgParamCodes::Processing:
-                    // @@TODO we may actually use this branch to report progress of manual operation on the MMU
-                    // The MMU sends e.g. X0 P27 after its restart when the user presses an MMU button to move the Selector
-                    // For now let's behave just like "finished"
-                case ResponseMsgParamCodes::Finished:
-                    errorCode = ErrorCode::OK;
-                    break;
-                default:
-                    errorCode = static_cast<ErrorCode>(rsp.paramValue);
-                    SendFINDAQuery(); // continue Idle state without restarting the communication
-                    return CommandError;
-                }
-                break;
-            default:
-                return ProtocolError;
+    }
+    return Finished;
+}
+
+StepStatus ProtocolLogic::IdleStep() {
+    switch (scopeState) {
+    case ScopeState::QuerySent: // check UART
+        // If we are accidentally in Idle and we receive something like "T0 P1" - that means the communication dropped out while a command was in progress.
+        // That causes no issues here, we just need to switch to Command processing and continue there from now on.
+        // The usual response in this case should be some command and "F" - finished - that confirms we are in an Idle state even on the MMU side.
+        switch (rsp.request.code) {
+        case RequestMsgCodes::Cut:
+        case RequestMsgCodes::Eject:
+        case RequestMsgCodes::Load:
+        case RequestMsgCodes::Mode:
+        case RequestMsgCodes::Tool:
+        case RequestMsgCodes::Unload:
+            if (rsp.paramCode != ResponseMsgParamCodes::Finished) {
+                return SwitchFromIdleToCommand();
             }
-            SendFINDAQuery();
-            return Processing;
             break;
-        case ScopeState::FINDAReqSent:
-            SendReadRegister(3, ScopeState::StatisticsSent);
-            scopeState = ScopeState::StatisticsSent;
-            return Processing;
-        case ScopeState::StatisticsSent:
-            failStatistics = rsp.paramValue;
-            scopeState = ScopeState::Ready;
-            return Processing;
-        case ScopeState::ButtonSent:
-            if (rsp.paramCode == ResponseMsgParamCodes::Accepted) {
-                // Button was accepted, decrement the retry.
-                mmu2.DecrementRetryAttempts();
+        case RequestMsgCodes::Reset:
+            // this one is kind of special
+            // we do not transfer to any "running" command (i.e. we stay in Idle),
+            // but in case there is an error reported we must make sure it gets propagated
+            switch (rsp.paramCode) {
+            case ResponseMsgParamCodes::Button:
+                // The user pushed a button on the MMU. Save it, do what we need to do
+                // to prepare, then pass it back to the MMU so it can work its magic.
+                buttonCode = static_cast<Buttons>(rsp.paramValue);
+                SendFINDAQuery();
+                return ButtonPushed;
+            case ResponseMsgParamCodes::Processing:
+                // @@TODO we may actually use this branch to report progress of manual operation on the MMU
+                // The MMU sends e.g. X0 P27 after its restart when the user presses an MMU button to move the Selector
+                // For now let's behave just like "finished"
+            case ResponseMsgParamCodes::Finished:
+                errorCode = ErrorCode::OK;
+                break;
+            default:
+                errorCode = static_cast<ErrorCode>(rsp.paramValue);
+                SendFINDAQuery(); // continue Idle state without restarting the communication
+                return CommandError;
             }
-            SendFINDAQuery();
             break;
         default:
             return ProtocolError;
         }
+        SendFINDAQuery();
+        return Processing;
+        break;
+    case ScopeState::FINDAReqSent:
+        SendReadRegister(4, ScopeState::StatisticsSent);
+        scopeState = ScopeState::StatisticsSent;
+        return Processing;
+    case ScopeState::StatisticsSent:
+        failStatistics = rsp.paramValue;
+        scopeState = ScopeState::Ready;
+        return Finished;
+    case ScopeState::ButtonSent:
+        if (rsp.paramCode == ResponseMsgParamCodes::Accepted) {
+            // Button was accepted, decrement the retry.
+            mmu2.DecrementRetryAttempts();
+        }
+        SendFINDAQuery();
+        break;
+    default:
+        return ProtocolError;
     }
-    
+
     // The "return Finished" in this state machine requires a bit of explanation:
     // The Idle state either did nothing (still waiting for the heartbeat timeout)
     // or just successfully received the answer to Q0, whatever that was.
@@ -443,9 +426,7 @@ ProtocolLogic::ProtocolLogic(MMU2Serial *uart)
     , lastFSensor((uint8_t)WhereIsFilament())
     , findaPressed(false)
     , failStatistics(0)
-    , mmuFwVersionMajor(0)
-    , mmuFwVersionMinor(0)
-    , mmuFwVersionBuild(0)
+    , mmuFwVersion { 0, 0, 0 }
 {}
 
 void ProtocolLogic::Start() {
@@ -480,7 +461,7 @@ void ProtocolLogic::EjectFilament(uint8_t slot) {
     PlanGenericRequest(RequestMsg(RequestMsgCodes::Eject, slot));
 }
 
-void ProtocolLogic::CutFilament(uint8_t slot){
+void ProtocolLogic::CutFilament(uint8_t slot) {
     PlanGenericRequest(RequestMsg(RequestMsgCodes::Cut, slot));
 }
 
@@ -488,28 +469,28 @@ void ProtocolLogic::ResetMMU() {
     PlanGenericRequest(RequestMsg(RequestMsgCodes::Reset, 0));
 }
 
-void ProtocolLogic::Button(uint8_t index){
+void ProtocolLogic::Button(uint8_t index) {
     PlanGenericRequest(RequestMsg(RequestMsgCodes::Button, index));
 }
 
-void ProtocolLogic::Home(uint8_t mode){
+void ProtocolLogic::Home(uint8_t mode) {
     PlanGenericRequest(RequestMsg(RequestMsgCodes::Home, mode));
 }
 
 void ProtocolLogic::PlanGenericRequest(RequestMsg rq) {
     plannedRq = rq;
-    if( ! ExpectsResponse() ){
+    if (!ExpectsResponse()) {
         ActivatePlannedRequest();
     } // otherwise wait for an empty window to activate the request
 }
 
-bool ProtocolLogic::ActivatePlannedRequest(){
-    if( plannedRq.code == RequestMsgCodes::Button ){
+bool ProtocolLogic::ActivatePlannedRequest() {
+    if (plannedRq.code == RequestMsgCodes::Button) {
         // only issue the button to the MMU and do not restart the state machines
         SendButton(plannedRq.value);
         plannedRq = RequestMsg(RequestMsgCodes::unknown, 0);
         return true;
-    } else if( plannedRq.code != RequestMsgCodes::unknown ){
+    } else if (plannedRq.code != RequestMsgCodes::unknown) {
         currentScope = Scope::Command;
         SetRequestMsg(plannedRq);
         plannedRq = RequestMsg(RequestMsgCodes::unknown, 0);
@@ -519,12 +500,12 @@ bool ProtocolLogic::ActivatePlannedRequest(){
     return false;
 }
 
-void ProtocolLogic::SwitchFromIdleToCommand(){
+StepStatus ProtocolLogic::SwitchFromIdleToCommand() {
     currentScope = Scope::Command;
     SetRequestMsg(rsp.request);
     // we are recovering from a communication drop out, the command is already running
     // and we have just received a response to a Q0 message about a command progress
-    CommandContinueFromIdle();
+    return ProcessCommandQueryResponse();
 }
 
 void ProtocolLogic::SwitchToIdle() {
@@ -533,7 +514,7 @@ void ProtocolLogic::SwitchToIdle() {
     IdleRestart();
 }
 
-void ProtocolLogic::SwitchFromStartToIdle(){
+void ProtocolLogic::SwitchFromStartToIdle() {
     state = State::Running;
     currentScope = Scope::Idle;
     IdleRestart();
@@ -541,24 +522,6 @@ void ProtocolLogic::SwitchFromStartToIdle(){
     scopeState = ScopeState::QuerySent;
 }
 
-StepStatus ProtocolLogic::ScopeStep(){
-    switch(currentScope){
-    case Scope::StartSeq:
-        return StartSeqStep();
-    case Scope::DelayedRestart:
-        return DelayedRestartStep();
-    case Scope::Idle:
-        return IdleStep();
-    case Scope::Command:
-        return CommandStep();
-    case Scope::Stopped:
-        return StoppedStep();
-    default:
-        break;
-    }
-    return Finished;
-}
-
 bool ProtocolLogic::Elapsed(uint32_t timeout) const {
     return _millis() >= (lastUARTActivityMs + timeout);
 }
@@ -567,12 +530,12 @@ void ProtocolLogic::RecordUARTActivity() {
     lastUARTActivityMs = _millis();
 }
 
-void ProtocolLogic::RecordReceivedByte(uint8_t c){
+void ProtocolLogic::RecordReceivedByte(uint8_t c) {
     lastReceivedBytes[lrb] = c;
-    lrb = (lrb+1) % lastReceivedBytes.size();
+    lrb = (lrb + 1) % lastReceivedBytes.size();
 }
 
-constexpr char NibbleToChar(uint8_t c){
+constexpr char NibbleToChar(uint8_t c) {
     switch (c) {
     case 0:
     case 1:
@@ -597,42 +560,46 @@ constexpr char NibbleToChar(uint8_t c){
     }
 }
 
-void ProtocolLogic::FormatLastReceivedBytes(char *dst){
-    for(uint8_t i = 0; i < lastReceivedBytes.size(); ++i){
-        uint8_t b = lastReceivedBytes[ (lrb-i-1) % lastReceivedBytes.size() ];
-        dst[i*3] = NibbleToChar(b >> 4);
-        dst[i*3+1] = NibbleToChar(b & 0xf);
-        dst[i*3+2] = ' ';
+void ProtocolLogic::FormatLastReceivedBytes(char *dst) {
+    for (uint8_t i = 0; i < lastReceivedBytes.size(); ++i) {
+        uint8_t b = lastReceivedBytes[(lrb - i - 1) % lastReceivedBytes.size()];
+        dst[i * 3] = NibbleToChar(b >> 4);
+        dst[i * 3 + 1] = NibbleToChar(b & 0xf);
+        dst[i * 3 + 2] = ' ';
     }
-    dst[ (lastReceivedBytes.size() - 1) * 3 + 2] = 0; // terminate properly
+    dst[(lastReceivedBytes.size() - 1) * 3 + 2] = 0; // terminate properly
 }
 
-void ProtocolLogic::FormatLastResponseMsgAndClearLRB(char *dst){
+void ProtocolLogic::FormatLastResponseMsgAndClearLRB(char *dst) {
     *dst++ = '<';
-    for(uint8_t i = 0; i < lrb; ++i){
-        uint8_t b = lastReceivedBytes[ i ];
-        if( b < 32 )b = '.';
-        if( b > 127 )b = '.';
+    for (uint8_t i = 0; i < lrb; ++i) {
+        uint8_t b = lastReceivedBytes[i];
+        if (b < 32)
+            b = '.';
+        if (b > 127)
+            b = '.';
         *dst++ = b;
     }
     *dst = 0; // terminate properly
-    lrb = 0; // reset the input buffer index in case of a clean message
+    lrb = 0;  // reset the input buffer index in case of a clean message
 }
 
-void ProtocolLogic::LogRequestMsg(const uint8_t *txbuff, uint8_t size){
+void ProtocolLogic::LogRequestMsg(const uint8_t *txbuff, uint8_t size) {
     constexpr uint_fast8_t rqs = modules::protocol::Protocol::MaxRequestSize() + 2;
     char tmp[rqs] = ">";
     static char lastMsg[rqs] = "";
-    for(uint8_t i = 0; i < size; ++i){
+    for (uint8_t i = 0; i < size; ++i) {
         uint8_t b = txbuff[i];
-        if( b < 32 )b = '.';
-        if( b > 127 )b = '.';
-        tmp[i+1] = b;
+        if (b < 32)
+            b = '.';
+        if (b > 127)
+            b = '.';
+        tmp[i + 1] = b;
     }
-    tmp[size+1] = '\n';
-    tmp[size+2] = 0;
-    if( !strncmp_P(tmp, PSTR(">S0*99.\n"), rqs) && !strncmp(lastMsg, tmp, rqs) ){
-        // @@TODO we skip the repeated request msgs for now 
+    tmp[size + 1] = '\n';
+    tmp[size + 2] = 0;
+    if (!strncmp_P(tmp, PSTR(">S0*99.\n"), rqs) && !strncmp(lastMsg, tmp, rqs)) {
+        // @@TODO we skip the repeated request msgs for now
         // to avoid spoiling the whole log just with ">S0" messages
         // especially when the MMU is not connected.
         // We'll lose the ability to see if the printer is actually
@@ -644,16 +611,16 @@ void ProtocolLogic::LogRequestMsg(const uint8_t *txbuff, uint8_t size){
     memcpy(lastMsg, tmp, rqs);
 }
 
-void ProtocolLogic::LogError(const char *reason_P){
+void ProtocolLogic::LogError(const char *reason_P) {
     char lrb[lastReceivedBytes.size() * 3];
     FormatLastReceivedBytes(lrb);
-    
+
     MMU2_ERROR_MSGRPGM(reason_P);
     SERIAL_ECHOPGM(", last bytes: ");
     SERIAL_ECHOLN(lrb);
 }
 
-void ProtocolLogic::LogResponse(){
+void ProtocolLogic::LogResponse() {
     char lrb[lastReceivedBytes.size()];
     FormatLastResponseMsgAndClearLRB(lrb);
     MMU2_ECHO_MSG(lrb);
@@ -661,7 +628,7 @@ void ProtocolLogic::LogResponse(){
 }
 
 StepStatus ProtocolLogic::SuppressShortDropOuts(const char *msg_P, StepStatus ss) {
-    if( dataTO.Record(ss) ){
+    if (dataTO.Record(ss)) {
         LogError(msg_P);
         return dataTO.InitialCause();
     } else {
@@ -685,7 +652,7 @@ StepStatus ProtocolLogic::HandleProtocolError() {
 }
 
 StepStatus ProtocolLogic::Step() {
-    if( ! ExpectsResponse() ){ // if not waiting for a response, activate a planned request immediately
+    if (!ExpectsResponse()) { // if not waiting for a response, activate a planned request immediately
         ActivatePlannedRequest();
     }
     auto currentStatus = ScopeStep();
@@ -697,12 +664,12 @@ StepStatus ProtocolLogic::Step() {
         // We are ok, switching to Idle if there is no potential next request planned.
         // But the trouble is we must report a finished command if the previous command has just been finished
         // i.e. only try to find some planned command if we just finished the Idle cycle
-        bool previousCommandFinished = currentScope == Scope::Command; // @@TODO this is a nasty hack :( 
-        if( ! ActivatePlannedRequest() ){ // if nothing is planned, switch to Idle
+        bool previousCommandFinished = currentScope == Scope::Command; // @@TODO this is a nasty hack :(
+        if (!ActivatePlannedRequest()) {                               // if nothing is planned, switch to Idle
             SwitchToIdle();
         } else {
             // if the previous cycle was Idle and now we have planned a new command -> avoid returning Finished
-            if( ! previousCommandFinished && currentScope == Scope::Command){
+            if (!previousCommandFinished && currentScope == Scope::Command) {
                 currentStatus = Processing;
             }
         }
@@ -736,13 +703,13 @@ StepStatus ProtocolLogic::Step() {
 }
 
 uint8_t ProtocolLogic::CommandInProgress() const {
-    if( currentScope != Scope::Command )
+    if (currentScope != Scope::Command)
         return 0;
     return (uint8_t)ReqMsg().code;
 }
 
-bool DropOutFilter::Record(StepStatus ss){
-    if( occurrences == maxOccurrences ){
+bool DropOutFilter::Record(StepStatus ss) {
+    if (occurrences == maxOccurrences) {
         cause = ss;
     }
     --occurrences;

Firmware/mmu2_protocol_logic.h

@@ -34,20 +34,19 @@ enum StepStatus : uint_fast8_t {
     MessageReady, ///< a message has been successfully decoded from the received bytes
     Finished,
     CommunicationTimeout, ///< the MMU failed to respond to a request within a specified time frame
-    ProtocolError, ///< bytes read from the MMU didn't form a valid response
-    CommandRejected, ///< the MMU rejected the command due to some other command in progress, may be the user is operating the MMU locally (button commands)
-    CommandError, ///< the command in progress stopped due to unrecoverable error, user interaction required
-    VersionMismatch, ///< the MMU reports its firmware version incompatible with our implementation
+    ProtocolError,        ///< bytes read from the MMU didn't form a valid response
+    CommandRejected,      ///< the MMU rejected the command due to some other command in progress, may be the user is operating the MMU locally (button commands)
+    CommandError,         ///< the command in progress stopped due to unrecoverable error, user interaction required
+    VersionMismatch,      ///< the MMU reports its firmware version incompatible with our implementation
     CommunicationRecovered,
-    ButtonPushed, ///< The MMU reported the user pushed one of its three buttons. 
+    ButtonPushed, ///< The MMU reported the user pushed one of its three buttons.
 };
 
-
-static constexpr uint32_t linkLayerTimeout = 2000; ///< default link layer communication timeout
+static constexpr uint32_t linkLayerTimeout = 2000;                 ///< default link layer communication timeout
 static constexpr uint32_t dataLayerTimeout = linkLayerTimeout * 3; ///< data layer communication timeout
-static constexpr uint32_t heartBeatPeriod = linkLayerTimeout / 2; ///< period of heart beat messages (Q0)
+static constexpr uint32_t heartBeatPeriod = linkLayerTimeout / 2;  ///< period of heart beat messages (Q0)
 
-static_assert( heartBeatPeriod < linkLayerTimeout && linkLayerTimeout < dataLayerTimeout, "Incorrect ordering of timeouts");
+static_assert(heartBeatPeriod < linkLayerTimeout && linkLayerTimeout < dataLayerTimeout, "Incorrect ordering of timeouts");
 
 ///< Filter of short consecutive drop outs which are recovered instantly
 class DropOutFilter {
@@ -55,17 +54,17 @@ class DropOutFilter {
     uint8_t occurrences;
 public:
     static constexpr uint8_t maxOccurrences = 10; // ideally set this to >8 seconds -> 12x heartBeatPeriod
-    static_assert (maxOccurrences > 1, "we should really silently ignore at least 1 comm drop out if recovered immediately afterwards");
+    static_assert(maxOccurrences > 1, "we should really silently ignore at least 1 comm drop out if recovered immediately afterwards");
     DropOutFilter() = default;
-    
+
     /// @returns true if the error should be reported to higher levels (max. number of consecutive occurrences reached)
     bool Record(StepStatus ss);
-    
+
     /// @returns the initial cause which started this drop out event
-    inline StepStatus InitialCause()const { return cause; }
-    
+    inline StepStatus InitialCause() const { return cause; }
+
     /// Rearms the object for further processing - basically call this once the MMU responds with something meaningful (e.g. S0 A2)
-    inline void Reset(){ occurrences = maxOccurrences; }
+    inline void Reset() { occurrences = maxOccurrences; }
 };
 
 /// Logic layer of the MMU vs. printer communication protocol
@@ -98,13 +97,13 @@ public:
 
     /// @returns the current/latest process code as reported by the MMU
     ProgressCode Progress() const { return progressCode; }
-    
+
     /// @returns the current/latest button code as reported by the MMU
     Buttons Button() const { return buttonCode; }
 
-    uint8_t CommandInProgress()const;
+    uint8_t CommandInProgress() const;
 
-    inline bool Running()const {
+    inline bool Running() const {
         return state == State::Running;
     }
 
@@ -117,20 +116,20 @@ public:
     }
 
     inline uint8_t MmuFwVersionMajor() const {
-        return mmuFwVersionMajor;
+        return mmuFwVersion[0];
     }
 
     inline uint8_t MmuFwVersionMinor() const {
-        return mmuFwVersionMinor;
+        return mmuFwVersion[1];
     }
 
-    inline uint8_t MmuFwVersionBuild() const {
-        return mmuFwVersionBuild;
+    inline uint8_t MmuFwVersionRevision() const {
+        return mmuFwVersion[2];
     }
 #ifndef UNITTEST
 private:
 #endif
-    StepStatus ExpectingMessage(uint32_t timeout);
+    StepStatus ExpectingMessage();
     void SendMsg(RequestMsg rq);
     void SwitchToIdle();
     StepStatus SuppressShortDropOuts(const char *msg_P, StepStatus ss);
@@ -144,9 +143,9 @@ private:
     void LogRequestMsg(const uint8_t *txbuff, uint8_t size);
     void LogError(const char *reason_P);
     void LogResponse();
-    void SwitchFromIdleToCommand();
+    StepStatus SwitchFromIdleToCommand();
     void SwitchFromStartToIdle();
-    
+
     enum class State : uint_fast8_t {
         Stopped,      ///< stopped for whatever reason
         InitSequence, ///< initial sequence running
@@ -170,17 +169,14 @@ private:
         Command
     };
     Scope currentScope;
-    
+
     // basic scope members
     /// @returns true if the state machine is waiting for a response from the MMU
-    bool ExpectsResponse()const { return scopeState != ScopeState::Ready && scopeState != ScopeState::Wait; }
-    
+    bool ExpectsResponse() const { return ((uint8_t)scopeState & (uint8_t)ScopeState::NotExpectsResponse) == 0; }
+
     /// Common internal states of the derived sub-automata
     /// General rule of thumb: *Sent states are waiting for a response from the MMU
-    enum class ScopeState : uint_fast8_t { 
-        Ready,
-        Wait,
-        
+    enum class ScopeState : uint_fast8_t {
         S0Sent, // beware - due to optimization reasons these SxSent must be kept one after another
         S1Sent,
         S2Sent,
@@ -191,23 +187,26 @@ private:
         FINDAReqSent,
         StatisticsSent,
         ButtonSent,
-        
-        ContinueFromIdle,
-        RecoveringProtocolError
+
+        // States which do not expect a message - MSb set
+        NotExpectsResponse = 0x80,
+        Wait = NotExpectsResponse + 1,
+        Ready = NotExpectsResponse + 2,
+        RecoveringProtocolError = NotExpectsResponse + 3,
     };
-    
+
     ScopeState scopeState; ///< internal state of the sub-automaton
-    
+
     /// @returns the status of processing of the FINDA query response
     /// @param finishedRV returned value in case the message was successfully received and processed
     /// @param nextState is a state where the state machine should transfer to after the message was successfully received and processed
     // StepStatus ProcessFINDAReqSent(StepStatus finishedRV, State nextState);
-    
+
     /// @returns the status of processing of the statistics query response
     /// @param finishedRV returned value in case the message was successfully received and processed
     /// @param nextState is a state where the state machine should transfer to after the message was successfully received and processed
     // StepStatus ProcessStatisticsReqSent(StepStatus finishedRV, State nextState);
-    
+
     /// Called repeatedly while waiting for a query (Q0) period.
     /// All event checks to report immediately from the printer to the MMU shall be done in this method.
     /// So far, the only such a case is the filament sensor, but there can be more like this in the future.
@@ -218,42 +217,45 @@ private:
     void SendButton(uint8_t btn);
     void SendVersion(uint8_t stage);
     void SendReadRegister(uint8_t index, ScopeState nextState);
-    
+
+    StepStatus ProcessVersionResponse(uint8_t stage);
+
     /// Top level split - calls the appropriate step based on current scope
     StepStatus ScopeStep();
-    
+
     static constexpr uint8_t maxRetries = 6;
     uint8_t retries;
-    
+
     void StartSeqRestart();
     void DelayedRestartRestart();
     void IdleRestart();
     void CommandRestart();
-    
+
     StepStatus StartSeqStep();
-    StepStatus DelayedRestartStep();
+    StepStatus DelayedRestartWait();
     StepStatus IdleStep();
+    StepStatus IdleWait();
     StepStatus CommandStep();
-    StepStatus StoppedStep(){ return Processing; }
-    
+    StepStatus CommandWait();
+    StepStatus StoppedStep() { return Processing; }
+
+    StepStatus ProcessCommandQueryResponse();
+
     inline void SetRequestMsg(RequestMsg msg) {
         rq = msg;
     }
-    void CommandContinueFromIdle(){
-        scopeState = ScopeState::ContinueFromIdle;
-    }
-    inline const RequestMsg &ReqMsg()const { return rq; }
+    inline const RequestMsg &ReqMsg() const { return rq; }
     RequestMsg rq = RequestMsg(RequestMsgCodes::unknown, 0);
-    
+
     /// Records the next planned state, "unknown" msg code if no command is planned.
     /// This is not intended to be a queue of commands to process, protocol_logic must not queue commands.
     /// It exists solely to prevent breaking the Request-Response protocol handshake -
     /// - during tests it turned out, that the commands from Marlin are coming in such an asynchronnous way, that
     /// we could accidentally send T2 immediately after Q0 without waiting for reception of response to Q0.
-    /// 
+    ///
     /// Beware, if Marlin manages to call PlanGenericCommand multiple times before a response comes,
     /// these variables will get overwritten by the last call.
-    /// However, that should not happen under normal circumstances as Marlin should wait for the Command to finish, 
+    /// However, that should not happen under normal circumstances as Marlin should wait for the Command to finish,
     /// which includes all responses (and error recovery if any).
     RequestMsg plannedRq;
 
@@ -263,7 +265,7 @@ private:
     bool ActivatePlannedRequest();
 
     uint32_t lastUARTActivityMs; ///< timestamp - last ms when something occurred on the UART
-    DropOutFilter dataTO; ///< Filter of short consecutive drop outs which are recovered instantly
+    DropOutFilter dataTO;        ///< Filter of short consecutive drop outs which are recovered instantly
 
     ResponseMsg rsp; ///< decoded response message from the MMU protocol
 
@@ -285,8 +287,8 @@ private:
     bool findaPressed;
     uint16_t failStatistics;
 
-    uint8_t mmuFwVersionMajor, mmuFwVersionMinor;
-    uint8_t mmuFwVersionBuild;
+    uint8_t mmuFwVersion[3];
+    uint16_t mmuFwVersionBuild;
 
     friend class ProtocolLogicPartBase;
     friend class Stopped;