Updated PIC port MS/TP to not always Reply-Postponed frame when sent Data-Expecting-Reply frame.

bacnet-stack/ports/pic18f6720/dlmstp.c

@@ -95,7 +95,6 @@ int dlmstp_send_pdu(
     int bytes_sent = 0;
     unsigned npdu_len = 0;
     uint8_t frame_type = 0;
-    uint8_t destination = 0;    /* destination address */
     BACNET_ADDRESS src;
     unsigned i = 0;     /* loop counter */
 
@@ -107,7 +106,7 @@ int dlmstp_send_pdu(
 
         /* load destination MAC address */
         if (dest && dest->mac_len == 1) {
-            destination = dest->mac[0];
+            MSTP_Port.TxDestination = dest->mac[0];
         } else {
             return -2;
         }
@@ -116,9 +115,13 @@ int dlmstp_send_pdu(
             return -4;
         }
         bytes_sent =
-            MSTP_Create_Frame((uint8_t *) & MSTP_Port.TxBuffer[0],
+            MSTP_Create_Frame(
-            sizeof(MSTP_Port.TxBuffer), MSTP_Port.TxFrameType, destination,
+            (uint8_t *) & MSTP_Port.TxBuffer[0],
-            MSTP_Port.This_Station, pdu, pdu_len);
+            sizeof(MSTP_Port.TxBuffer),
+            MSTP_Port.TxFrameType,
+            MSTP_Port.TxDestination,
+            MSTP_Port.This_Station,
+            pdu, pdu_len);
         MSTP_Port.TxLength = bytes_sent;
         MSTP_Port.TxReady = true;
         MSTP_Packets++;

bacnet-stack/ports/pic18f6720/mstp.c

@@ -551,6 +551,113 @@ void MSTP_Receive_Frame_FSM(
     return;
 }
 
+static bool mstp_compare_data_expecting_reply(
+    uint8_t * request_pdu,
+    uint16_t request_pdu_len,
+    uint8_t src_address,
+    uint8_t * reply_pdu,
+    uint16_t reply_pdu_len,
+    uint8_t dest_address)
+{
+    uint16_t offset;
+    /* One way to check the message is to compare NPDU
+       src, dest, along with the APDU type, invoke id.
+       Seems a bit overkill */
+    struct DER_compare_t {
+        BACNET_NPDU_DATA npdu_data;
+        BACNET_ADDRESS address;
+        uint8_t pdu_type;
+        uint8_t invoke_id;
+        uint8_t service_choice;
+    };
+    struct DER_compare_t request;
+    struct DER_compare_t reply;
+
+    /* decode the request data */
+    request.address.mac[0] = src_address;
+    request.address.mac_len = 1;
+    offset =
+        npdu_decode(&request_pdu[0], NULL, &request.address,
+        &request.npdu_data);
+    if (request.npdu_data.network_layer_message) {
+        return false;
+    }
+    request.pdu_type = request_pdu[offset] & 0xF0;
+    if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) {
+        return false;
+    }
+    request.invoke_id = request_pdu[offset + 2];
+    /* segmented message? */
+    if (request_pdu[offset] & BIT3)
+        request.service_choice = request_pdu[offset + 5];
+    else
+        request.service_choice = request_pdu[offset + 3];
+    /* decode the reply data */
+    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) {
+        return false;
+    }
+    /* reply could be a lot of things:
+       confirmed, simple ack, abort, reject, error */
+    reply.pdu_type = reply_pdu[offset] & 0xF0;
+    switch (reply.pdu_type) {
+        case PDU_TYPE_CONFIRMED_SERVICE_REQUEST:
+            reply.invoke_id = reply_pdu[offset + 2];
+            /* segmented message? */
+            if (reply_pdu[offset] & BIT3)
+                reply.service_choice = reply_pdu[offset + 5];
+            else
+                reply.service_choice = reply_pdu[offset + 3];
+            break;
+        case PDU_TYPE_SIMPLE_ACK:
+            reply.invoke_id = reply_pdu[offset + 1];
+            reply.service_choice = reply_pdu[offset + 2];
+            break;
+        case PDU_TYPE_COMPLEX_ACK:
+            reply.invoke_id = reply_pdu[offset + 1];
+            /* segmented message? */
+            if (reply_pdu[offset] & BIT3)
+                reply.service_choice = reply_pdu[offset + 4];
+            else
+                reply.service_choice = reply_pdu[offset + 2];
+            break;
+        case PDU_TYPE_ERROR:
+            reply.invoke_id = reply_pdu[offset + 1];
+            reply.service_choice = reply_pdu[offset + 2];
+            break;
+        case PDU_TYPE_REJECT:
+        case PDU_TYPE_ABORT:
+            reply.invoke_id = reply_pdu[offset + 1];
+            break;
+        default:
+            return false;
+    }
+    if (request.invoke_id != reply.invoke_id) {
+        return false;
+    }
+    /* these services don't have service choice included */
+    if ((reply.pdu_type != PDU_TYPE_REJECT) &&
+        (reply.pdu_type != PDU_TYPE_ABORT)) {
+        if (request.service_choice != reply.service_choice) {
+            return false;
+        }
+    }
+    if (request.npdu_data.protocol_version != reply.npdu_data.protocol_version) {
+        return false;
+    }
+    if (request.npdu_data.priority != reply.npdu_data.priority) {
+        return false;
+    }
+    if (!bacnet_address_same(&request.address, &reply.address)) {
+        return false;
+    }
+
+    return true;
+}
+
 /* returns true if we need to transition immediately */
 bool MSTP_Master_Node_FSM(
     volatile struct mstp_port_struct_t * mstp_port)
@@ -563,6 +670,7 @@ bool MSTP_Master_Node_FSM(
     uint16_t my_timeout = 10, ns_timeout = 0;
     /* transition immediately to the next state */
     bool transition_now = false;
+    bool matched = false;
 #if PRINT_ENABLED_MASTER
     static MSTP_MASTER_STATE master_state = MSTP_MASTER_STATE_INITIALIZE;
 #endif
@@ -1032,13 +1140,20 @@ 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:
-#if 0
+            if (mstp_port->TxReady) {
-            /* FIXME: we always defer the reply to be safe */
+                /* Compare the APDU type received and
-            /* FIXME: if we knew the APDU type received, we could
+                   see if the message is that same APDU type
-               see if the next message was that same APDU type
+                   along with the matching src/dest and invoke ID */
-               along with the matching src/dest and invoke ID */
+                matched =
-            if ((mstp_port->SilenceTimer <= Treply_delay) &&
+                    mstp_compare_data_expecting_reply(
-                mstp_port->TxReady) {
+                        &mstp_port->InputBuffer[0],
+                        mstp_port->DataLength,
+                        mstp_port->SourceAddress,
+                        &mstp_port->TxBuffer[0],
+                        mstp_port->TxLength,
+                        mstp_port->TxDestination);
+            }
+            if (matched && mstp_port->TxReady) {
                 /* Reply */
                 /* If a reply is available from the higher layers  */
                 /* within Treply_delay after the reception of the  */
@@ -1046,17 +1161,12 @@ bool MSTP_Master_Node_FSM(
                 /* (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. */
-                if ((mstp_port->FrameType ==
+                RS485_Send_Frame(mstp_port,
-                        FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY)
+                    (uint8_t *) & mstp_port->TxBuffer[0],
-                    && (mstp_port->TxReady)) {
+                    mstp_port->TxLength);
-                    RS485_Send_Frame(mstp_port,
+                mstp_port->TxReady = false;
-                        (uint8_t *) & mstp_port->TxBuffer[0],
+                mstp_port->master_state = MSTP_MASTER_STATE_IDLE;
-                        mstp_port->TxLength);
+            } else {
-                    mstp_port->TxReady = false;
-                    mstp_port->master_state = MSTP_MASTER_STATE_IDLE;
-                }
-            } else
-#endif
                 /* DeferredReply */
                 /* If no reply will be available from the higher layers */
                 /* within Treply_delay after the reception of the */
@@ -1064,9 +1174,8 @@ bool MSTP_Master_Node_FSM(
                 /* used to determine this is a local matter), */
                 /* then an immediate reply is not possible. */
                 /* Any reply shall wait until this node receives the token. */
-                /* Call MSTP_Create_And_Send_Frame to transmit a Reply Postponed frame, */
+                /* Call MSTP_Create_And_Send_Frame to transmit a */
-                /* and enter the IDLE state. */
+                /* Reply Postponed frame, and enter the IDLE state. */
-            {
                 MSTP_Create_And_Send_Frame(mstp_port,
                     FRAME_TYPE_REPLY_POSTPONED, mstp_port->SourceAddress,
                     mstp_port->This_Station, NULL, 0);

bacnet-stack/ports/pic18f6720/mstp.h

@@ -150,6 +150,7 @@ struct mstp_port_struct_t {
     /* This array is only used for APDU messages */
     uint8_t TxBuffer[MAX_MPDU];
     unsigned TxLength;
+    uint8_t TxDestination;
     bool TxReady;       /* true if ready to be sent or received */
     uint8_t TxFrameType;        /* type of message - needed by MS/TP */
 };