Updated the Datalink Data-Expecting-Reply for the at91sam7s and atmega168 projects.

bacnet-stack/ports/at91sam7s/dlmstp.c

@@ -32,6 +32,7 @@
 #include "rs485.h"
 #include "npdu.h"
 #include "apdu.h"
+#include "bacaddr.h"
 #include "bits.h"
 /* This file has been customized for use with the AT91SAM7S-EK */
 #include "board.h"
@@ -49,23 +50,6 @@ static volatile struct mstp_port_struct_t MSTP_Port;
 static uint8_t TxBuffer[MAX_MPDU];
 static uint8_t RxBuffer[MAX_MPDU];
 
-void dlmstp_copy_bacnet_address(BACNET_ADDRESS * dest, BACNET_ADDRESS * src)
-{
-    int i = 0;
-
-    if (dest && src) {
-        dest->mac_len = src->mac_len;
-        for (i = 0; i < MAX_MAC_LEN; i++) {
-            dest->mac[i] = src->mac[i];
-        }
-        dest->net = src->net;
-        dest->len = src->len;
-        for (i = 0; i < MAX_MAC_LEN; i++) {
-            dest->adr[i] = src->adr[i];
-        }
-    }
-}
-
 bool dlmstp_init(char *ifname)
 {
     (void)ifname;
@@ -112,7 +96,7 @@ int dlmstp_send_pdu(BACNET_ADDRESS * dest, /* destination address */
         for (i = 0; i < pdu_len; i++) {
             Transmit_Packet.pdu[i] = pdu[i];
         }
-        dlmstp_copy_bacnet_address(&Transmit_Packet.address, dest);
+        bacnet_address_copy(&Transmit_Packet.address, dest);
         bytes_sent = sizeof(Transmit_Packet);
         Transmit_Packet.ready = true;
     }
@@ -164,7 +148,7 @@ uint16_t dlmstp_receive(
             for (i = 0; i < Receive_Packet.pdu_len; i++) {
                 pdu[i] = Receive_Packet.pdu[i];
             }
-            dlmstp_copy_bacnet_address(src, &Receive_Packet.address);
+            bacnet_address_copy(src, &Receive_Packet.address);
             pdu_len = Receive_Packet.pdu_len;
         }
         Receive_Packet.ready = false;
@@ -256,30 +240,6 @@ uint16_t MSTP_Get_Send(
     return pdu_len;
 }
 
-bool dlmstp_same_bacnet_address(BACNET_ADDRESS * dest, BACNET_ADDRESS * src)
-{
-    int i = 0;
-
-    if (!dest || !src)
-        return false;
-    if (dest->mac_len != src->mac_len)
-        return false;
-    for (i = 0; i < dest->mac_len; i++) {
-        if (dest->mac[i] != src->mac[i])
-            return false;
-    }
-    if (dest->net != src->net)
-        return false;
-    if (dest->len != src->len)
-        return false;
-    for (i = 0; i < dest->len; i++) {
-        if (dest->adr[i] != src->adr[i])
-            return false;
-    }
-    
-    return true;
-}
-
 bool dlmstp_compare_data_expecting_reply(
     uint8_t *request_pdu,
     uint16_t request_pdu_len,
@@ -321,7 +281,7 @@ bool dlmstp_compare_data_expecting_reply(
     else
         request.service_choice = request_pdu[offset+3];
     /* decode the reply data */
-    dlmstp_copy_bacnet_address(&reply.address, dest_address);
+    bacnet_address_copy(&reply.address, dest_address);
     offset = npdu_decode(&reply_pdu[0], 
         &reply.address, NULL, &reply.npdu_data);
     if (reply.npdu_data.network_layer_message) {
@@ -382,7 +342,7 @@ bool dlmstp_compare_data_expecting_reply(
     if (request.npdu_data.priority != reply.npdu_data.priority) {
         return false;
     }
-    if (!dlmstp_same_bacnet_address(&request.address, &reply.address)) {
+    if (!bacnet_address_same(&request.address, &reply.address)) {
         return false;
     }
     

bacnet-stack/ports/at91sam7s/makefile

@@ -59,6 +59,7 @@ CORESRC = ../../npdu.c \
 	../../bacint.c \
 	../../apdu.c \
 	../../bacdcode.c \
+	../../bacaddr.c \
 	../../bacstr.c \
 	../../abort.c \
 	../../bacerror.c \

bacnet-stack/ports/atmega168/Makefile

@@ -17,6 +17,9 @@ CSRC = main.c \
 	timer.c \
 	rs485.c \
 	dlmstp.c \
+	../../npdu.c \
+	../../bacint.c \
+	../../bacaddr.c \
 	../../mstp.c \
 	../../crc.c
 
@@ -28,7 +31,7 @@ DEMOSRC = h_rp.c \
 	../../demo/handler/h_rd.c \
 	../../demo/handler/h_dcc.c
 
-CORESRC = ../../npdu.c \
+CORESRC =  \
 	../../bacint.c \
 	../../apdu.c \
 	../../bacdcode.c \

bacnet-stack/ports/atmega168/dlmstp.c

@@ -31,6 +31,8 @@
 #include "dlmstp.h"
 #include "rs485.h"
 #include "npdu.h"
+#include "bits.h"
+#include "bacaddr.h"
 
 /* This file has been customized for use with the ATmega168 */
 #include "hardware.h"
@@ -47,30 +49,6 @@ static uint8_t RxBuffer[MAX_MPDU];
 static DLMSTP_PACKET Receive_Packet;
 static DLMSTP_PACKET Transmit_Packet;
 
-#define INCREMENT_AND_LIMIT_UINT16(x) {if (x < 0xFFFF) x++;}
-
-void dlmstp_millisecond_timer(void)
-{
-    INCREMENT_AND_LIMIT_UINT16(MSTP_Port.SilenceTimer);
-}
-
-void dlmstp_copy_bacnet_address(BACNET_ADDRESS * dest, BACNET_ADDRESS * src)
-{
-    int i = 0;
-
-    if (src && dest) {
-        src->mac_len = dest->mac_len;
-        for (i = 0; i < MAX_MAC_LEN; i++) {
-            src->mac[i] = dest->mac[i];
-        }
-        src->net = dest->net;
-        src->len = dest->len;
-        for (i = 0; i < MAX_MAC_LEN; i++) {
-            src->adr[i] = dest->adr[i];
-        }
-    }
-}
-
 bool dlmstp_init(char *ifname)
 {
     (void)ifname;
@@ -115,7 +93,7 @@ int dlmstp_send_pdu(BACNET_ADDRESS * dest, /* destination address */
         for (i = 0; i < pdu_len; i++) {
             Transmit_Packet.pdu[i] = pdu[i];
         }
-        dlmstp_copy_bacnet_address(&Transmit_Packet.address, dest);
+        bacnet_address_copy(&Transmit_Packet.address, dest);
         bytes_sent = sizeof(Transmit_Packet);
     }
 
@@ -165,7 +143,7 @@ uint16_t dlmstp_receive(
             for (i = 0; i < Receive_Packet.pdu_len; i++) {
                 pdu[i] = Receive_Packet.pdu[i];
             }
-            dlmstp_copy_bacnet_address(src, &Receive_Packet.address);
+            bacnet_address_copy(src, &Receive_Packet.address);
             pdu_len = Receive_Packet.pdu_len;
         }
         Receive_Packet.ready = false;
@@ -225,33 +203,188 @@ uint16_t MSTP_Put_Receive(
 /* for the MS/TP state machine to use for getting data to send */
 /* Return: amount of PDU data */
 uint16_t MSTP_Get_Send(
-    uint8_t src, /* source MS/TP address for creating packet */
+    volatile struct mstp_port_struct_t *mstp_port,
-    uint8_t * pdu, /* data to send */
+    unsigned timeout) /* milliseconds to wait for a packet */
-    uint16_t max_pdu, /* amount of space available */
+{
+    uint8_t destination = 0;    /* destination address */
+
+    if (!Transmit_Packet.ready) {
+        return 0;
+    }
+    /* load destination MAC address */
+    if (Transmit_Packet.address.mac_len == 1) {
+        destination = Transmit_Packet.address.mac[0];
+    } else {
+        return 0;
+    }
+    if ((8 /* header len */  + Transmit_Packet.pdu_len) > MAX_MPDU) {
+        return 0;
+    }
+    /* convert the PDU into the MSTP Frame */
+    return MSTP_Create_Frame(
+        &mstp_port->OutputBuffer[0], /* <-- loading this */
+        mstp_port->OutputBufferSize,
+        Transmit_Packet.frame_type,
+        destination, 
+        mstp_port->This_Station,
+        &Transmit_Packet.pdu[0],
+        Transmit_Packet.pdu_len);
+}
+
+bool dlmstp_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,
+    BACNET_ADDRESS *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 */
+    bacnet_address_copy(&reply.address, dest_address);
+    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;
+    }
+    /* these don't have service choice included */
+    if ((reply.pdu_type == PDU_TYPE_REJECT) ||
+        (reply.pdu_type == PDU_TYPE_ABORT)) {
+        if (request.invoke_id != reply.invoke_id) {
+            return false;
+        }
+    } else {
+        if (request.invoke_id != reply.invoke_id) {
+            return false;
+        }
+        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;
+}
+
+/* Get the reply to a DATA_EXPECTING_REPLY frame, or nothing */
+uint16_t MSTP_Get_Reply(
+    volatile struct mstp_port_struct_t *mstp_port,
     unsigned timeout) /* milliseconds to wait for a packet */
 {
     uint16_t pdu_len = 0; /* return value */
     uint8_t destination = 0;    /* destination address */
+    bool matched = false;
 
-    if (Transmit_Packet.ready) {
+    if (!Transmit_Packet.ready) {
-        /* load destination MAC address */
+        return 0;
-        if (Transmit_Packet.address.mac_len == 1) {
-            destination = Transmit_Packet.address.mac[0];
-        } else {
-            return 0;
-        }
-        if ((8 /* header len */  + Transmit_Packet.pdu_len) > MAX_MPDU) {
-            return 0;
-        }
-        /* convert the PDU into the MSTP Frame */
-        pdu_len = MSTP_Create_Frame(
-            pdu, /* <-- loading this */
-            max_pdu,
-            Transmit_Packet.frame_type,
-            destination, src,
-            &Transmit_Packet.pdu[0],
-            Transmit_Packet.pdu_len);
     }
+    /* 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;
+    }
+    /* does destination match source? */
+    if (mstp_port->SourceAddress != destination) {
+        return 0;
+    }
+    /* is this the reply to the DER? */
+    matched = dlmstp_compare_data_expecting_reply(
+        &mstp_port->InputBuffer[0],
+        mstp_port->DataLength,
+        mstp_port->SourceAddress,
+        &Transmit_Packet.pdu[0],
+        Transmit_Packet.pdu_len,
+        &Transmit_Packet.address);
+    if (!matched)
+        return 0;
+    /* convert the PDU into the MSTP Frame */
+    pdu_len = MSTP_Create_Frame(
+        &mstp_port->OutputBuffer[0], /* <-- loading this */
+        mstp_port->OutputBufferSize,
+        Transmit_Packet.frame_type,
+        destination, 
+        mstp_port->This_Station,
+        &Transmit_Packet.pdu[0],
+        Transmit_Packet.pdu_len);
+    Transmit_Packet.ready = false;
 
     return pdu_len;
 }

bacnet-stack/ports/atmega168/main.c

@@ -89,6 +89,16 @@ void task_milliseconds(void)
     }
 }
 
+void apdu_handler(BACNET_ADDRESS * src,     /* source address */
+        uint8_t * apdu,         /* APDU data */
+        uint16_t pdu_len)      /* for confirmed messages */
+{
+    (void)src;
+    (void)apdu;
+    (void)pdu_len;
+}
+
+
 static uint8_t PDUBuffer[MAX_MPDU];
 int main(void)
 {