diff --git a/bacnet-stack/ports/bdk-atxx4-mstp/dlmstp.c b/bacnet-stack/ports/bdk-atxx4-mstp/dlmstp.c
index 40b4e275..42ae539c 100644
--- a/bacnet-stack/ports/bdk-atxx4-mstp/dlmstp.c
+++ b/bacnet-stack/ports/bdk-atxx4-mstp/dlmstp.c
@@ -627,9 +627,6 @@ static void MSTP_Receive_Frame_FSM(
                         MSTP_Flag.ReceivedInvalidFrame = true;
                     }
                     Receive_State = MSTP_RECEIVE_STATE_IDLE;
-                } else {
-                    MSTP_Flag.ReceivedInvalidFrame = true;
-                    Receive_State = MSTP_RECEIVE_STATE_IDLE;
                 }
             }
             break;
@@ -1207,7 +1204,9 @@ uint16_t dlmstp_receive(
         (MSTP_Flag.ReceivedInvalidFrame == false)) {
         MSTP_Receive_Frame_FSM();
     }
-    /* only do master state machine while rx is idle */
+    if (MSTP_Flag.ReceivedValidFrameNotForUs) {
+        MSTP_Flag.ReceivedValidFrameNotForUs = false;
+    }
     if (Receive_State == MSTP_RECEIVE_STATE_IDLE) {
         while (MSTP_Master_Node_FSM()) {
             /* do nothing while some states fast transition */