Returned Tusage_delay to the MS/TP state machines to fix excessive Reply-Posponed messages when received packet is decoupled from immediate handling. Some BACnet routers do not handle Reply-Postponed correctly and drop the subsequent valid reply.

Added Slave Node FSM to linux/win32 MS/TP layer.
Added PDU queue to Linux MS/TP datalink layer.

bacnet-stack/include/mstp.h

@@ -177,6 +177,8 @@ extern "C" {
     bool MSTP_Master_Node_FSM(
         volatile struct mstp_port_struct_t
         *mstp_port);
+    void MSTP_Slave_Node_FSM(
+        volatile struct mstp_port_struct_t * mstp_port);
 
     /* returns true if line is active */
     bool MSTP_Line_Active(

bacnet-stack/ports/at91sam7s/dlmstp.c

@@ -1090,7 +1090,6 @@ static bool MSTP_Master_Node_FSM(
             /* BACnet Data Expecting Reply, a Test_Request, or  */
             /* a proprietary frame that expects a reply is received. */
         case MSTP_MASTER_STATE_ANSWER_DATA_REQUEST:
-            /* Note: we could wait for up to Treply_delay */
             matched = false;
             if (!Ringbuf_Empty(&PDU_Queue)) {
                 pkt = (struct mstp_pdu_packet *) Ringbuf_Get_Front(&PDU_Queue);
@@ -1117,7 +1116,9 @@ static bool MSTP_Master_Node_FSM(
                 MSTP_Send_Frame(frame_type, pkt->destination_mac, This_Station,
                     (uint8_t *) & pkt->buffer[0], pkt->length);
                 Master_State = MSTP_MASTER_STATE_IDLE;
-            } else {
+                /* clear our flag we were holding for comparison */
+                MSTP_Flag.ReceivedValidFrame = false;
+            } else if (rs485_silence_time_elapsed(Treply_delay)) {
                 /* DeferredReply */
                 /* If no reply will be available from the higher layers */
                 /* within Treply_delay after the reception of the */
@@ -1130,9 +1131,9 @@ static bool MSTP_Master_Node_FSM(
                 MSTP_Send_Frame(FRAME_TYPE_REPLY_POSTPONED, SourceAddress,
                     This_Station, NULL, 0);
                 Master_State = MSTP_MASTER_STATE_IDLE;
+                /* clear our flag we were holding for comparison */
+                MSTP_Flag.ReceivedValidFrame = false;
             }
-            /* clear our flag we were holding for comparison */
-            MSTP_Flag.ReceivedValidFrame = false;
             break;
         default:
             Master_State = MSTP_MASTER_STATE_IDLE;
@@ -1150,6 +1151,7 @@ static void MSTP_Slave_Node_FSM(
     /* did the frame in the queue match the last request? */
     bool matched = false;
 
+    Master_State = MSTP_MASTER_STATE_IDLE;
     if (MSTP_Flag.ReceivedInvalidFrame == true) {
         /* ReceivedInvalidFrame */
         /* invalid frame was received */
@@ -1175,13 +1177,7 @@ static void MSTP_Slave_Node_FSM(
                 break;
         }
     } else if (MSTP_Flag.ReceivePacketPending) {
-        if (Ringbuf_Empty(&PDU_Queue)) {
+        if (!Ringbuf_Empty(&PDU_Queue)) {
-            /* 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.ReceivePacketPending = false;
-        } else {
             pkt = (struct mstp_pdu_packet *) Ringbuf_Pop_Front(&PDU_Queue);
             matched =
                 dlmstp_compare_data_expecting_reply(&InputBuffer[0],
@@ -1204,10 +1200,16 @@ static void MSTP_Slave_Node_FSM(
                 }
                 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.ReceivePacketPending = false;
+        } else if (rs485_silence_time_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;
         }
     }
 }

bacnet-stack/ports/bdk-atxx4-mstp/dlmstp.c

@@ -1117,7 +1117,6 @@ static bool MSTP_Master_Node_FSM(
             /* BACnet Data Expecting Reply, a Test_Request, or  */
             /* a proprietary frame that expects a reply is received. */
         case MSTP_MASTER_STATE_ANSWER_DATA_REQUEST:
-            /* Note: we could wait for up to Treply_delay */
             matched = false;
             if (!Ringbuf_Empty(&PDU_Queue)) {
                 pkt = (struct mstp_pdu_packet *) Ringbuf_Get_Front(&PDU_Queue);
@@ -1144,7 +1143,9 @@ static bool MSTP_Master_Node_FSM(
                 MSTP_Send_Frame(frame_type, pkt->destination_mac, This_Station,
                     (uint8_t *) & pkt->buffer[0], pkt->length);
                 Master_State = MSTP_MASTER_STATE_IDLE;
-            } else {
+                /* clear our flag we were holding for comparison */
+                MSTP_Flag.ReceivedValidFrame = false;
+            } else if (rs485_silence_time_elapsed(Treply_delay)) {
                 /* DeferredReply */
                 /* If no reply will be available from the higher layers */
                 /* within Treply_delay after the reception of the */
@@ -1157,9 +1158,9 @@ static bool MSTP_Master_Node_FSM(
                 MSTP_Send_Frame(FRAME_TYPE_REPLY_POSTPONED, SourceAddress,
                     This_Station, NULL, 0);
                 Master_State = MSTP_MASTER_STATE_IDLE;
+                /* clear our flag we were holding for comparison */
+                MSTP_Flag.ReceivedValidFrame = false;
             }
-            /* clear our flag we were holding for comparison */
-            MSTP_Flag.ReceivedValidFrame = false;
             break;
         default:
             Master_State = MSTP_MASTER_STATE_IDLE;

bacnet-stack/ports/linux/dlmstp.c

@@ -37,6 +37,7 @@
 #include "rs485.h"
 #include "npdu.h"
 #include "bits.h"
+#include "ringbuf.h"
 /* OS Specific include */
 #include "net.h"
 
@@ -47,7 +48,6 @@ uint16_t MSTP_Packets = 0;
 
 /* packet queues */
 static DLMSTP_PACKET Receive_Packet;
-static DLMSTP_PACKET Transmit_Packet;
 /* mechanism to wait for a packet */
 /*
 static RT_COND Receive_Packet_Flag;
@@ -72,6 +72,19 @@ static volatile struct mstp_port_struct_t MSTP_Port;
 /* buffers needed by mstp port struct */
 static uint8_t TxBuffer[MAX_MPDU];
 static uint8_t RxBuffer[MAX_MPDU];
+/* data structure for MS/TP PDU Queue */
+struct mstp_pdu_packet {
+    bool data_expecting_reply;
+    uint8_t destination_mac;
+    uint16_t length;
+    uint8_t buffer[MAX_MPDU];
+};
+/* count must be a power of 2 for ringbuf library */
+#ifndef MSTP_PDU_PACKET_COUNT
+#define MSTP_PDU_PACKET_COUNT 8
+#endif
+static struct mstp_pdu_packet PDU_Buffer[MSTP_PDU_PACKET_COUNT];
+static RING_BUFFER PDU_Queue;
 /* The minimum time without a DataAvailable or ReceiveError event */
 /* that a node must wait for a station to begin replying to a */
 /* confirmed request: 255 milliseconds. (Implementations may use */
@@ -138,23 +151,18 @@ int dlmstp_send_pdu(
     unsigned pdu_len)
 {       /* number of bytes of data */
     int bytes_sent = 0;
+    struct mstp_pdu_packet *pkt;
     unsigned i = 0;
 
-    if (!Transmit_Packet.ready) {
+    pkt = (struct mstp_pdu_packet *) Ringbuf_Alloc(&PDU_Queue);
-        if (npdu_data->data_expecting_reply) {
+    if (pkt) {
-            Transmit_Packet.frame_type =
+        pkt->data_expecting_reply = npdu_data->data_expecting_reply;
-                FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY;
-        } else {
-            Transmit_Packet.frame_type =
-                FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY;
-        }
-        Transmit_Packet.pdu_len = pdu_len;
         for (i = 0; i < pdu_len; i++) {
-            Transmit_Packet.pdu[i] = pdu[i];
+            pkt->buffer[i] = pdu[i];
         }
-        bacnet_address_copy(&Transmit_Packet.address, dest);
+        pkt->length = pdu_len;
-        bytes_sent = pdu_len + MAX_HEADER;
+        pkt->destination_mac = dest->mac[0];
-        Transmit_Packet.ready = true;
+        bytes_sent = pdu_len;
     }
 
     return bytes_sent;
@@ -233,7 +241,11 @@ static void *dlmstp_master_fsm_task(
             }
         }
         if (run_master) {
-            MSTP_Master_Node_FSM(&MSTP_Port);
+            if (MSTP_Port.This_Station <= DEFAULT_MAX_MASTER) {
+                MSTP_Master_Node_FSM(&MSTP_Port);
+            } else if (MSTP_Port.This_Station < 255) {
+                MSTP_Slave_Node_FSM(&MSTP_Port);
+            }
         }
     }
 
@@ -295,27 +307,28 @@ uint16_t MSTP_Get_Send(
     unsigned timeout)
 {       /* milliseconds to wait for a packet */
     uint16_t pdu_len = 0;
-    uint8_t destination = 0;    /* destination address */
+    uint8_t frame_type = 0;
+    struct mstp_pdu_packet *pkt;
 
     (void) timeout;
-    if (!Transmit_Packet.ready) {
+    if (Ringbuf_Empty(&PDU_Queue)) {
         return 0;
     }
-    /* load destination MAC address */
+    pkt = (struct mstp_pdu_packet *) Ringbuf_Pop_Front(&PDU_Queue);
-    if (Transmit_Packet.address.mac_len == 1) {
+    if (pkt->data_expecting_reply) {
-        destination = Transmit_Packet.address.mac[0];
+        frame_type = FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY;
     } else {
-        return 0;
+        frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY;
-    }
-    if ((MAX_HEADER + Transmit_Packet.pdu_len) > MAX_MPDU) {
-        return 0;
     }
     /* convert the PDU into the MSTP Frame */
-    pdu_len = MSTP_Create_Frame(&mstp_port->OutputBuffer[0],    /* <-- loading this */
+    pdu_len = MSTP_Create_Frame(
-        mstp_port->OutputBufferSize, Transmit_Packet.frame_type, destination,
+        &mstp_port->OutputBuffer[0],    /* <-- loading this */
-        mstp_port->This_Station, &Transmit_Packet.pdu[0],
+        mstp_port->OutputBufferSize,
-        Transmit_Packet.pdu_len);
+        frame_type,
-    Transmit_Packet.ready = false;
+        pkt->destination_mac,
+        mstp_port->This_Station,
+        (uint8_t *) & pkt->buffer[0],
+        pkt->length);
 
     return pdu_len;
 }
@@ -326,7 +339,7 @@ static bool dlmstp_compare_data_expecting_reply(
     uint8_t src_address,
     uint8_t * reply_pdu,
     uint16_t reply_pdu_len,
-    BACNET_ADDRESS * dest_address)
+    uint8_t dest_address)
 {
     uint16_t offset;
     /* One way to check the message is to compare NPDU
@@ -352,10 +365,20 @@ static bool dlmstp_compare_data_expecting_reply(
         npdu_decode(&request_pdu[0], NULL, &request.address,
         &request.npdu_data);
     if (request.npdu_data.network_layer_message) {
+#if PRINT_ENABLED
+        fprintf(stderr,
+            "DLMSTP: DER Compare failed: "
+            "Request is Network message.\n");
+#endif
         return false;
     }
     request.pdu_type = request_pdu[offset] & 0xF0;
     if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) {
+#if PRINT_ENABLED
+        fprintf(stderr,
+            "DLMSTP: DER Compare failed: "
+            "Not Confirmed Request.\n");
+#endif
         return false;
     }
     request.invoke_id = request_pdu[offset + 2];
@@ -366,10 +389,16 @@ static bool dlmstp_compare_data_expecting_reply(
         request.service_choice = request_pdu[offset + 3];
     }
     /* decode the reply data */
-    bacnet_address_copy(&reply.address, dest_address);
+    reply.address.mac[0] = dest_address;
+    reply.address.mac_len = 1;
     offset =
         npdu_decode(&reply_pdu[0], &reply.address, NULL, &reply.npdu_data);
     if (reply.npdu_data.network_layer_message) {
+#if PRINT_ENABLED
+        fprintf(stderr,
+            "DLMSTP: DER Compare failed: "
+            "Reply is Network message.\n");
+#endif
         return false;
     }
     /* reply could be a lot of things:
@@ -413,23 +442,53 @@ static bool dlmstp_compare_data_expecting_reply(
     if ((reply.pdu_type == PDU_TYPE_REJECT) ||
         (reply.pdu_type == PDU_TYPE_ABORT)) {
         if (request.invoke_id != reply.invoke_id) {
+#if PRINT_ENABLED
+        fprintf(stderr,
+            "DLMSTP: DER Compare failed: "
+            "Invoke ID mismatch.\n");
+#endif
             return false;
         }
     } else {
         if (request.invoke_id != reply.invoke_id) {
+#if PRINT_ENABLED
+        fprintf(stderr,
+            "DLMSTP: DER Compare failed: "
+            "Invoke ID mismatch.\n");
+#endif
             return false;
         }
         if (request.service_choice != reply.service_choice) {
+#if PRINT_ENABLED
+        fprintf(stderr,
+            "DLMSTP: DER Compare failed: "
+            "Service choice mismatch.\n");
+#endif
             return false;
         }
     }
     if (request.npdu_data.protocol_version != reply.npdu_data.protocol_version) {
+#if PRINT_ENABLED
+        fprintf(stderr,
+            "DLMSTP: DER Compare failed: "
+            "NPDU Protocol Version mismatch.\n");
+#endif
         return false;
     }
     if (request.npdu_data.priority != reply.npdu_data.priority) {
+#if PRINT_ENABLED
+        fprintf(stderr,
+            "DLMSTP: DER Compare failed: "
+            "NPDU Priority mismatch.\n");
+#endif
         return false;
     }
     if (!bacnet_address_same(&request.address, &reply.address)) {
+#if PRINT_ENABLED
+        fprintf(stderr,
+            "DLMSTP: DER Compare failed: "
+            "BACnet Address mismatch.\n");
+#endif
         return false;
     }
 
@@ -442,37 +501,42 @@ uint16_t MSTP_Get_Reply(
     unsigned timeout)
 {       /* milliseconds to wait for a packet */
     uint16_t pdu_len = 0;       /* return value */
-    uint8_t destination = 0;    /* destination address */
     bool matched = false;
+    uint8_t frame_type = 0;
+    struct mstp_pdu_packet *pkt;
 
     (void) timeout;
-    if (!Transmit_Packet.ready) {
+    if (Ringbuf_Empty(&PDU_Queue)) {
-        return 0;
-    }
-    /* load destination MAC address */
-    if (Transmit_Packet.address.mac_len == 1) {
-        destination = Transmit_Packet.address.mac[0];
-    } else {
-        return 0;
-    }
-    if ((MAX_HEADER + Transmit_Packet.pdu_len) > MAX_MPDU) {
         return 0;
     }
+    pkt = (struct mstp_pdu_packet *) Ringbuf_Get_Front(&PDU_Queue);
     /* is this the reply to the DER? */
     matched =
-        dlmstp_compare_data_expecting_reply(&mstp_port->InputBuffer[0],
+        dlmstp_compare_data_expecting_reply(
-        mstp_port->DataLength, mstp_port->SourceAddress,
+        &mstp_port->InputBuffer[0],
-        &Transmit_Packet.pdu[0], Transmit_Packet.pdu_len,
+        mstp_port->DataLength,
-        &Transmit_Packet.address);
+        mstp_port->SourceAddress,
+        (uint8_t *) & pkt->buffer[0],
+        pkt->length,
+        pkt->destination_mac);
     if (!matched) {
         return 0;
     }
+    pkt = (struct mstp_pdu_packet *) Ringbuf_Pop_Front(&PDU_Queue);
+    if (pkt->data_expecting_reply) {
+        frame_type = FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY;
+    } else {
+        frame_type = FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY;
+    }
     /* convert the PDU into the MSTP Frame */
-    pdu_len = MSTP_Create_Frame(&mstp_port->OutputBuffer[0],    /* <-- loading this */
+    pdu_len = MSTP_Create_Frame(
-        mstp_port->OutputBufferSize, Transmit_Packet.frame_type, destination,
+        &mstp_port->OutputBuffer[0],    /* <-- loading this */
-        mstp_port->This_Station, &Transmit_Packet.pdu[0],
+        mstp_port->OutputBufferSize,
-        Transmit_Packet.pdu_len);
+        frame_type,
-    Transmit_Packet.ready = false;
+        pkt->destination_mac,
+        mstp_port->This_Station,
+        (uint8_t *) & pkt->buffer[0],
+        pkt->length);
 
     return pdu_len;
 }
@@ -610,6 +674,10 @@ bool dlmstp_init(
     unsigned long hThread = 0;
     int rv = 0;
 
+    /* initialize PDU queue */
+    Ringbuf_Init(&PDU_Queue, (uint8_t *) & PDU_Buffer,
+        sizeof(struct mstp_pdu_packet),
+        MSTP_PDU_PACKET_COUNT);
     /* initialize packet queue */
     Receive_Packet.ready = false;
     Receive_Packet.pdu_len = 0;

bacnet-stack/ports/linux/rs485.c

@@ -231,31 +231,36 @@ void RS485_Check_UART_Data(
     int n;
 
     if (mstp_port->ReceiveError == true) {
-        /* wait for state machine to clear this */
+        /* do nothing but wait for state machine to clear the error */
-        /*mstp_port->ReceiveError=false; */
+        /* burning time, so wait a longer time */
-        return;
+        waiter.tv_sec = 0;
-    }
+        waiter.tv_usec = 5000;
-    /* wait for state machine to read from the DataRegister */
+    } else if (mstp_port->DataAvailable == false) {
-    if (mstp_port->DataAvailable == false) {
+        /* wait for state machine to read from the DataRegister */
-        /* check for data */
         if (FIFO_Count(&Rx_FIFO) > 0) {
+            /* data is available */
             mstp_port->DataRegister = FIFO_Get(&Rx_FIFO);
             mstp_port->DataAvailable = true;
+            /* FIFO is giving data - don't wait very long */
+            waiter.tv_sec = 0;
+            waiter.tv_usec = 10;
         } else {
-            FD_ZERO (&input);
+            /* FIFO is empty - wait a longer time */
-            FD_SET (RS485_Handle, &input);
             waiter.tv_sec = 0;
             waiter.tv_usec = 5000;
-            n = select (RS485_Handle + 1, &input, NULL, NULL, &waiter);
-            if (n < 0) {
-                return;
-            }
-            if (FD_ISSET(RS485_Handle, &input)) {
-                n = read(RS485_Handle, buf, sizeof(buf));
-                FIFO_Add(&Rx_FIFO, &buf[0], n);
-            }
         }
     }
+    /* grab bytes and stuff them into the FIFO every time */
+    FD_ZERO (&input);
+    FD_SET (RS485_Handle, &input);
+    n = select (RS485_Handle + 1, &input, NULL, NULL, &waiter);
+    if (n < 0) {
+        return;
+    }
+    if (FD_ISSET(RS485_Handle, &input)) {
+        n = read(RS485_Handle, buf, sizeof(buf));
+        FIFO_Add(&Rx_FIFO, &buf[0], n);
+    }
 }
 
 void RS485_Cleanup(

bacnet-stack/ports/rtos32/mstp.c

@@ -114,7 +114,7 @@ const unsigned Tpostdrive = 15;
 /* The maximum time a node may wait after reception of a frame that expects */
 /* a reply before sending the first octet of a reply or Reply Postponed */
 /* frame: 250 milliseconds. */
-const uint16_t Treply_delay = 10;
+const uint16_t Treply_delay = 250;
 
 /* The minimum time without a DataAvailable or ReceiveError event */
 /* that a node must wait for a station to begin replying to a */

bacnet-stack/ports/stm32f10x/dlmstp.c

@@ -1206,7 +1206,6 @@ static bool MSTP_Master_Node_FSM(
             /* BACnet Data Expecting Reply, a Test_Request, or  */
             /* a proprietary frame that expects a reply is received. */
         case MSTP_MASTER_STATE_ANSWER_DATA_REQUEST:
-            /* Note: we could wait for up to Treply_delay */
             matched = false;
             if (!Ringbuf_Empty(&PDU_Queue)) {
                 pkt = (struct mstp_pdu_packet *) Ringbuf_Get_Front(&PDU_Queue);
@@ -1233,7 +1232,9 @@ static bool MSTP_Master_Node_FSM(
                 MSTP_Send_Frame(frame_type, pkt->destination_mac, This_Station,
                     (uint8_t *) & pkt->buffer[0], pkt->length);
                 Master_State = MSTP_MASTER_STATE_IDLE;
-            } else {
+                /* clear our flag we were holding for comparison */
+                MSTP_Flag.ReceivedValidFrame = false;
+            } else if (rs485_silence_time_elapsed(Treply_delay)) {
                 /* DeferredReply */
                 /* If no reply will be available from the higher layers */
                 /* within Treply_delay after the reception of the */
@@ -1246,9 +1247,9 @@ static bool MSTP_Master_Node_FSM(
                 MSTP_Send_Frame(FRAME_TYPE_REPLY_POSTPONED, SourceAddress,
                     This_Station, NULL, 0);
                 Master_State = MSTP_MASTER_STATE_IDLE;
+                /* clear our flag we were holding for comparison */
+                MSTP_Flag.ReceivedValidFrame = false;
             }
-            /* clear our flag we were holding for comparison */
-            MSTP_Flag.ReceivedValidFrame = false;
             break;
         default:
             Master_State = MSTP_MASTER_STATE_IDLE;

bacnet-stack/src/mstp.c

@@ -142,7 +142,7 @@ static inline void printf_master(
 /* The maximum time a node may wait after reception of a frame that expects */
 /* a reply before sending the first octet of a reply or Reply Postponed */
 /* frame: 250 milliseconds. */
-#define Treply_delay 10
+#define Treply_delay 250
 
 /* Repeater turnoff delay. The duration of a continuous logical one state */
 /* at the active input port of an MS/TP repeater after which the repeater */
@@ -1051,7 +1051,6 @@ bool MSTP_Master_Node_FSM(
             /* The ANSWER_DATA_REQUEST state is entered when a  */
             /* BACnet Data Expecting Reply, a Test_Request, or  */
             /* a proprietary frame that expects a reply is received. */
-            /* FIXME: we could wait for up to Treply_delay */
             length = (unsigned) MSTP_Get_Reply(mstp_port, 0);
             if (length > 0) {
                 /* Reply */
@@ -1065,7 +1064,9 @@ bool MSTP_Master_Node_FSM(
                     (uint8_t *) & mstp_port->OutputBuffer[0],
                     (uint16_t) length);
                 mstp_port->master_state = MSTP_MASTER_STATE_IDLE;
-            } else {
+                /* clear our flag we were holding for comparison */
+                mstp_port->ReceivedValidFrame = false;
+            } else if (mstp_port->SilenceTimer() > Treply_delay) {
                 /* DeferredReply */
                 /* If no reply will be available from the higher layers */
                 /* within Treply_delay after the reception of the */
@@ -1079,9 +1080,9 @@ bool MSTP_Master_Node_FSM(
                     FRAME_TYPE_REPLY_POSTPONED, mstp_port->SourceAddress,
                     mstp_port->This_Station, NULL, 0);
                 mstp_port->master_state = MSTP_MASTER_STATE_IDLE;
+                /* clear our flag we were holding for comparison */
+                mstp_port->ReceivedValidFrame = false;
             }
-            /* clear our flag we were holding for comparison */
-            mstp_port->ReceivedValidFrame = false;
             break;
         default:
             mstp_port->master_state = MSTP_MASTER_STATE_IDLE;
@@ -1091,6 +1092,70 @@ bool MSTP_Master_Node_FSM(
     return transition_now;
 }
 
+void MSTP_Slave_Node_FSM(
+    volatile struct mstp_port_struct_t * mstp_port)
+{
+    unsigned length = 0;
+
+    mstp_port->master_state = MSTP_MASTER_STATE_IDLE;
+    if (mstp_port->ReceivedInvalidFrame == true) {
+        /* ReceivedInvalidFrame */
+        /* invalid frame was received */
+        mstp_port->ReceivedInvalidFrame = false;
+    } else if (mstp_port->ReceivedValidFrame) {
+        switch (mstp_port->FrameType) {
+            case FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY:
+                if (mstp_port->DestinationAddress != MSTP_BROADCAST_ADDRESS) {
+                    /* The ANSWER_DATA_REQUEST state is entered when a  */
+                    /* BACnet Data Expecting Reply, a Test_Request, or  */
+                    /* a proprietary frame that expects a reply is received. */
+                    length = (unsigned) MSTP_Get_Reply(mstp_port, 0);
+                    if (length > 0) {
+                        /* 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_Create_And_Send_Frame to transmit the reply frame  */
+                        /* and enter the IDLE state to wait for the next frame. */
+                        RS485_Send_Frame(mstp_port,
+                            (uint8_t *) & mstp_port->OutputBuffer[0],
+                            (uint16_t) length);
+                        /* clear our flag we were holding for comparison */
+                        mstp_port->ReceivedValidFrame = false;
+                    } else if (mstp_port->SilenceTimer() > 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_port->ReceivedValidFrame = false;
+                    }
+                } else {
+                    mstp_port->ReceivedValidFrame = false;
+                }
+                break;
+            case FRAME_TYPE_TEST_REQUEST:
+                mstp_port->ReceivedValidFrame = false;
+                MSTP_Create_And_Send_Frame(
+                    mstp_port,
+                    FRAME_TYPE_TEST_RESPONSE,
+                    mstp_port->SourceAddress,
+                    mstp_port->This_Station,
+                    &mstp_port->InputBuffer[0],
+                    mstp_port->DataLength);
+                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_port->ReceivedValidFrame = false;
+                break;
+        }
+    }
+}
+
 /* note: This_Station assumed to be set with the MAC address */
 /* note: Nmax_info_frames assumed to be set (default=1) */
 /* note: Nmax_master assumed to be set (default=127) */