Fix MSTP slave FSM for Data-Expecting-Reply frames (#538)

* Fix MSTP subordinate nodes Data-Expecting-Reply handling

---------

Co-authored-by: Steve Karg <skarg@users.sourceforge.net>

ports/at91sam7s/dlmstp.c

@@ -1276,7 +1276,10 @@ uint16_t dlmstp_receive(BACNET_ADDRESS *src, /* source address */
     }
     /* if there is a packet that needs processed, do it now. */
     if (MSTP_Flag.ReceivePacketPending) {
-        MSTP_Flag.ReceivePacketPending = false;
+        if (This_Station <= 127) {
+            /* master nodes clear immediately */
+            MSTP_Flag.ReceivePacketPending = false;
+        }
         pdu_len = DataLength;
         src->mac_len = 1;
         src->mac[0] = SourceAddress;

ports/bdk-atxx4-mstp/dlmstp.c

@@ -1309,7 +1309,10 @@ uint16_t dlmstp_receive(BACNET_ADDRESS *src, /* source address */
     }
     /* if there is a packet that needs processed, do it now. */
     if (MSTP_Flag.ReceivePacketPending) {
-        MSTP_Flag.ReceivePacketPending = false;
+        if (This_Station <= 127) {
+            /* master nodes clear immediately */
+            MSTP_Flag.ReceivePacketPending = false;
+        }
         pdu_len = DataLength;
         src->mac_len = 1;
         src->mac[0] = SourceAddress;

ports/stm32f10x/dlmstp.c

@@ -733,68 +733,70 @@ static void MSTP_Receive_Frame_FSM(void)
 
 static void MSTP_Slave_Node_FSM(void)
 {
-    /* destination address */
+    Master_State = MSTP_MASTER_STATE_IDLE;
-    uint8_t destination;
+    if (MSTP_Flag.ReceivedInvalidFrame == true) {
-    /* source address */
-    uint8_t source;
-    /* any data to be sent - may be null */
-    uint8_t *data;
-    /* amount of data to be sent - may be 0 */
-    uint16_t data_len;
-    /* packet from the PDU Queue */
-    struct dlmstp_packet *pkt;
-
-    if (MSTP_Flag.ReceivedInvalidFrame) {
         /* ReceivedInvalidFrame */
         /* invalid frame was received */
         MSTP_Flag.ReceivedInvalidFrame = false;
     } else if (MSTP_Flag.ReceivedValidFrame) {
+        MSTP_Flag.ReceivedValidFrame = false;
         switch (FrameType) {
             case FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY:
                 if (DestinationAddress != MSTP_BROADCAST_ADDRESS) {
-                    /* The ANSWER_DATA_REQUEST state is entered when a  */
+                    /* indicate successful reception to the higher layers  */
-                    /* BACnet Data Expecting Reply, a Test_Request, or  */
+                    MSTP_Flag.ReceivePacketPending = true;
-                    /* a proprietary frame that expects a reply is received. */
-                    pkt = (struct dlmstp_packet *)Ringbuf_Peek(&PDU_Queue);
-                    if (pkt != NULL) {
-                        MSTP_Send_Frame(pkt->frame_type, pkt->address.mac[0],
-                            This_Station, (uint8_t *)&pkt->pdu[0],
-                            pkt->pdu_len);
-                        Master_State = MSTP_MASTER_STATE_IDLE;
-                        /* clear our flag we were holding for comparison */
-                        MSTP_Flag.ReceivedValidFrame = false;
-                        /* clear the queue */
-                        (void)Ringbuf_Pop(&PDU_Queue, NULL);
-                    } else if (rs485_silence_elapsed(Treply_delay)) {
-                        /* If no reply will be available from the higher layers
-                           within Treply_delay after the reception of the final
-                           octet of the requesting frame (the mechanism used
-                           to determine this is a local matter), then no reply
-                           is possible. */
-                        MSTP_Flag.ReceivedValidFrame = false;
-                    }
-                } else {
-                    /* no reply when addressed as Broadcast */
-                    MSTP_Flag.ReceivedValidFrame = false;
                 }
                 break;
             case FRAME_TYPE_TEST_REQUEST:
-                MSTP_Flag.ReceivedValidFrame = false;
+                MSTP_Send_Frame(FRAME_TYPE_TEST_RESPONSE, SourceAddress,
-                destination = SourceAddress;
+                    This_Station, &InputBuffer[0], DataLength);
-                source = This_Station;
-                data = &InputBuffer[0];
-                data_len = DataLength;
-                MSTP_Send_Frame(FRAME_TYPE_TEST_RESPONSE, destination, source,
-                    data, data_len);
                 break;
             case FRAME_TYPE_TOKEN:
             case FRAME_TYPE_POLL_FOR_MASTER:
             case FRAME_TYPE_TEST_RESPONSE:
             case FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY:
             default:
-                MSTP_Flag.ReceivedValidFrame = false;
                 break;
         }
+    } else if (MSTP_Flag.ReceivePacketPending) {
+        if (!Ringbuf_Empty(&PDU_Queue)) {
+            /* packet from the PDU Queue */
+            struct mstp_pdu_packet *pkt;
+            /* did the frame in the queue match the last request? */
+            bool matched;
+
+            pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&PDU_Queue);
+            matched = dlmstp_compare_data_expecting_reply(&InputBuffer[0],
+                DataLength, SourceAddress, &pkt->buffer[0], pkt->length,
+                pkt->destination_mac);
+            if (matched) {
+                /* Reply */
+                /* If a reply is available from the higher layers  */
+                /* within Treply_delay after the reception of the  */
+                /* final octet of the requesting frame  */
+                /* (the mechanism used to determine this is a local matter), */
+                /* then call MSTP_Send_Frame to transmit the reply frame  */
+                /* and enter the IDLE state to wait for the next frame. */
+                uint8_t frame_type;
+                if (pkt->data_expecting_reply) {
+                    frame_type = FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY;
+                } else {
+                    frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY;
+                }
+                MSTP_Send_Frame(frame_type, pkt->destination_mac, This_Station,
+                    (uint8_t *)&pkt->buffer[0], pkt->length);
+                (void)Ringbuf_Pop(&PDU_Queue, NULL);
+            }
+            /* clear our flag we were holding for comparison */
+            MSTP_Flag.ReceivePacketPending = false;
+        } else if ((rs485_silence_elapsed(Treply_delay))) {
+            /* If no reply will be available from the higher layers
+               within Treply_delay after the reception of the final octet
+               of the requesting frame (the mechanism used to determine
+               this is a local matter), then no reply is possible. */
+            /* clear our flag we were holding for comparison */
+            MSTP_Flag.ReceivePacketPending = false;
+        }
     }
 }
 
@@ -1645,7 +1647,10 @@ uint16_t dlmstp_receive(BACNET_ADDRESS *src, /* source address */
     if (This_Station != 255) {
         /* if there is a packet that needs processed, do it now. */
         if (MSTP_Flag.ReceivePacketPending) {
-            MSTP_Flag.ReceivePacketPending = false;
+            if (This_Station <= 127) {
+                /* master nodes clear immediately */
+                MSTP_Flag.ReceivePacketPending = false;
+            }
             pdu_len = DataLength;
             src->mac_len = 1;
             src->mac[0] = SourceAddress;

ports/stm32f4xx/dlmstp.c

@@ -776,68 +776,64 @@ void log_master_state(MSTP_MASTER_STATE state)
 
 static void MSTP_Slave_Node_FSM(void)
 {
-    /* destination address */
+    Master_State = MSTP_MASTER_STATE_IDLE;
-    uint8_t destination;
+    if (MSTP_Flag.ReceivedInvalidFrame == true) {
-    /* source address */
-    uint8_t source;
-    /* any data to be sent - may be null */
-    uint8_t *data;
-    /* amount of data to be sent - may be 0 */
-    uint16_t data_len;
-    /* packet from the PDU Queue */
-    struct dlmstp_packet *pkt;
-
-    if (MSTP_Flag.ReceivedInvalidFrame) {
         /* ReceivedInvalidFrame */
         /* invalid frame was received */
         MSTP_Flag.ReceivedInvalidFrame = false;
     } else if (MSTP_Flag.ReceivedValidFrame) {
+        MSTP_Flag.ReceivedValidFrame = false;
         switch (FrameType) {
             case FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY:
                 if (DestinationAddress != MSTP_BROADCAST_ADDRESS) {
-                    /* The ANSWER_DATA_REQUEST state is entered when a  */
+                    /* indicate successful reception to the higher layers  */
-                    /* BACnet Data Expecting Reply, a Test_Request, or  */
+                    MSTP_Flag.ReceivePacketPending = true;
-                    /* a proprietary frame that expects a reply is received. */
-                    pkt = (struct dlmstp_packet *)Ringbuf_Peek(&PDU_Queue);
-                    if (pkt != NULL) {
-                        MSTP_Send_Frame(pkt->frame_type, pkt->address.mac[0],
-                            This_Station, (uint8_t *)&pkt->pdu[0],
-                            pkt->pdu_len);
-                        Master_State = MSTP_MASTER_STATE_IDLE;
-                        /* clear our flag we were holding for comparison */
-                        MSTP_Flag.ReceivedValidFrame = false;
-                        /* clear the queue */
-                        (void)Ringbuf_Pop(&PDU_Queue, NULL);
-                    } else if (rs485_silence_elapsed(Treply_delay)) {
-                        /* If no reply will be available from the higher layers
-                           within Treply_delay after the reception of the final
-                           octet of the requesting frame (the mechanism used
-                           to determine this is a local matter), then no reply
-                           is possible. */
-                        MSTP_Flag.ReceivedValidFrame = false;
-                    }
-                } else {
-                    /* no reply when addressed as Broadcast */
-                    MSTP_Flag.ReceivedValidFrame = false;
                 }
                 break;
             case FRAME_TYPE_TEST_REQUEST:
-                MSTP_Flag.ReceivedValidFrame = false;
+                MSTP_Send_Frame(FRAME_TYPE_TEST_RESPONSE, SourceAddress,
-                destination = SourceAddress;
+                    This_Station, &InputBuffer[0], DataLength);
-                source = This_Station;
-                data = &InputBuffer[0];
-                data_len = DataLength;
-                MSTP_Send_Frame(FRAME_TYPE_TEST_RESPONSE, destination, source,
-                    data, data_len);
                 break;
             case FRAME_TYPE_TOKEN:
             case FRAME_TYPE_POLL_FOR_MASTER:
             case FRAME_TYPE_TEST_RESPONSE:
             case FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY:
             default:
-                MSTP_Flag.ReceivedValidFrame = false;
                 break;
         }
+    } else if (MSTP_Flag.ReceivePacketPending) {
+        if (!Ringbuf_Empty(&PDU_Queue)) {
+            /* packet from the PDU Queue */
+            struct dlmstp_packet *pkt;
+            /* did the frame in the queue match the last request? */
+            bool matched;
+
+            pkt = (struct dlmstp_packet *)Ringbuf_Peek(&PDU_Queue);
+            matched = dlmstp_compare_data_expecting_reply(&InputBuffer[0],
+                DataLength, SourceAddress, &pkt->pdu[0], pkt->pdu_len,
+                pkt->address.mac[0]);
+            if (matched) {
+                /* Reply */
+                /* If a reply is available from the higher layers  */
+                /* within Treply_delay after the reception of the  */
+                /* final octet of the requesting frame  */
+                /* (the mechanism used to determine this is a local matter), */
+                /* then call MSTP_Send_Frame to transmit the reply frame  */
+                /* and enter the IDLE state to wait for the next frame. */
+                MSTP_Send_Frame(pkt->frame_type, pkt->address.mac[0],
+                    This_Station, pkt->pdu, pkt->pdu_len);
+                (void)Ringbuf_Pop(&PDU_Queue, NULL);
+            }
+            /* clear our flag we were holding for comparison */
+            MSTP_Flag.ReceivePacketPending = false;
+        } else if ((rs485_silence_elapsed(Treply_delay))) {
+            /* If no reply will be available from the higher layers
+               within Treply_delay after the reception of the final octet
+               of the requesting frame (the mechanism used to determine
+               this is a local matter), then no reply is possible. */
+            /* clear our flag we were holding for comparison */
+            MSTP_Flag.ReceivePacketPending = false;
+        }
     }
 }
 
@@ -1511,7 +1507,10 @@ uint16_t dlmstp_receive(
     }
     /* if there is a packet that needs processed, do it now. */
     if (MSTP_Flag.ReceivePacketPending) {
-        MSTP_Flag.ReceivePacketPending = false;
+        if (This_Station <= 127) {
+            /* master nodes clear immediately */
+            MSTP_Flag.ReceivePacketPending = false;
+        }
         Statistics.receive_pdu_counter++;
         pdu_len = DataLength;
         src->mac_len = 1;

ports/xplained/dlmstp.c

@@ -634,75 +634,70 @@ void log_master_state(MSTP_MASTER_STATE state)
 
 static void MSTP_Slave_Node_FSM(void)
 {
-    /* destination address */
+    Master_State = MSTP_MASTER_STATE_IDLE;
-    uint8_t destination;
+    if (MSTP_Flag.ReceivedInvalidFrame == true) {
-    /* source address */
-    uint8_t source;
-    /* any data to be sent - may be null */
-    uint8_t *data;
-    /* amount of data to be sent - may be 0 */
-    uint16_t data_len;
-    /* packet from the PDU Queue */
-    struct mstp_pdu_packet *pkt;
-
-    if (MSTP_Flag.ReceivedInvalidFrame) {
         /* ReceivedInvalidFrame */
         /* invalid frame was received */
         MSTP_Flag.ReceivedInvalidFrame = false;
     } else if (MSTP_Flag.ReceivedValidFrame) {
+        MSTP_Flag.ReceivedValidFrame = false;
         switch (FrameType) {
             case FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY:
                 if (DestinationAddress != MSTP_BROADCAST_ADDRESS) {
-                    /* The ANSWER_DATA_REQUEST state is entered when a  */
+                    /* indicate successful reception to the higher layers  */
-                    /* BACnet Data Expecting Reply, a Test_Request, or  */
+                    MSTP_Flag.ReceivePacketPending = true;
-                    /* a proprietary frame that expects a reply is received. */
-                    pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&PDU_Queue);
-                    if (pkt != NULL) {
-                        uint8_t frame_type;
-                        if (pkt->data_expecting_reply) {
-                            frame_type = FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY;
-                        } else {
-                            frame_type =
-                                FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY;
-                        }
-                        MSTP_Send_Frame(frame_type, pkt->destination_mac,
-                            This_Station, (uint8_t *)&pkt->buffer[0],
-                            pkt->length);
-                        Master_State = MSTP_MASTER_STATE_IDLE;
-                        /* clear our flag we were holding for comparison */
-                        MSTP_Flag.ReceivedValidFrame = false;
-                        /* clear the queue */
-                        (void)Ringbuf_Pop(&PDU_Queue, NULL);
-                    } else if (rs485_silence_elapsed(Treply_delay)) {
-                        /* If no reply will be available from the higher layers
-                           within Treply_delay after the reception of the final
-                           octet of the requesting frame (the mechanism used
-                           to determine this is a local matter), then no reply
-                           is possible. */
-                        MSTP_Flag.ReceivedValidFrame = false;
-                    }
-                } else {
-                    /* no reply when addressed as Broadcast */
-                    MSTP_Flag.ReceivedValidFrame = false;
                 }
                 break;
             case FRAME_TYPE_TEST_REQUEST:
-                MSTP_Flag.ReceivedValidFrame = false;
+                MSTP_Send_Frame(FRAME_TYPE_TEST_RESPONSE, SourceAddress,
-                destination = SourceAddress;
+                    This_Station, &InputBuffer[0], DataLength);
-                source = This_Station;
-                data = &InputBuffer[0];
-                data_len = DataLength;
-                MSTP_Send_Frame(FRAME_TYPE_TEST_RESPONSE, destination, source,
-                    data, data_len);
                 break;
             case FRAME_TYPE_TOKEN:
             case FRAME_TYPE_POLL_FOR_MASTER:
             case FRAME_TYPE_TEST_RESPONSE:
             case FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY:
             default:
-                MSTP_Flag.ReceivedValidFrame = false;
                 break;
         }
+    } else if (MSTP_Flag.ReceivePacketPending) {
+        if (!Ringbuf_Empty(&PDU_Queue)) {
+            /* packet from the PDU Queue */
+            struct mstp_pdu_packet *pkt;
+            /* did the frame in the queue match the last request? */
+            bool matched;
+
+            pkt = (struct mstp_pdu_packet *)Ringbuf_Peek(&PDU_Queue);
+            matched = dlmstp_compare_data_expecting_reply(&InputBuffer[0],
+                DataLength, SourceAddress, &pkt->buffer[0], pkt->length,
+                pkt->destination_mac);
+            if (matched) {
+                /* Reply */
+                /* If a reply is available from the higher layers  */
+                /* within Treply_delay after the reception of the  */
+                /* final octet of the requesting frame  */
+                /* (the mechanism used to determine this is a local matter), */
+                /* then call MSTP_Send_Frame to transmit the reply frame  */
+                /* and enter the IDLE state to wait for the next frame. */
+                uint8_t frame_type;
+                if (pkt->data_expecting_reply) {
+                    frame_type = FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY;
+                } else {
+                    frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY;
+                }
+                MSTP_Send_Frame(frame_type, pkt->destination_mac, This_Station,
+                    (uint8_t *)&pkt->buffer[0], pkt->length);
+                (void)Ringbuf_Pop(&PDU_Queue, NULL);
+            }
+            /* clear our flag we were holding for comparison */
+            MSTP_Flag.ReceivePacketPending = false;
+        } else if ((rs485_silence_elapsed(Treply_delay))) {
+            /* If no reply will be available from the higher layers
+               within Treply_delay after the reception of the final octet
+               of the requesting frame (the mechanism used to determine
+               this is a local matter), then no reply is possible. */
+            /* clear our flag we were holding for comparison */
+            MSTP_Flag.ReceivePacketPending = false;
+        }
     }
 }
 
@@ -1370,7 +1365,10 @@ uint16_t dlmstp_receive(
     }
     /* if there is a packet that needs processed, do it now. */
     if (MSTP_Flag.ReceivePacketPending) {
-        MSTP_Flag.ReceivePacketPending = false;
+        if (This_Station <= 127) {
+            /* master nodes clear immediately */
+            MSTP_Flag.ReceivePacketPending = false;
+        }
         pdu_len = DataLength;
         src->mac_len = 1;
         src->mac[0] = SourceAddress;