Simplified the WhoIsRouterToNetwork and InitializeRouter demo.

bacnet-stack/demo/initrouter/main.c

@@ -40,6 +40,8 @@
 #include "device.h"
 #include "datalink.h"
 /* some demo stuff needed */
+#define DEBUG_ENABLED 0
+#include "debug.h"
 #include "filename.h"
 #include "handlers.h"
 #include "client.h"
@@ -57,7 +59,7 @@ static BACNET_ROUTER_PORT *Target_Router_Port_List;
 
 static bool Error_Detected = false;
 
-void MyAbortHandler(
+static void MyAbortHandler(
     BACNET_ADDRESS * src,
     uint8_t invoke_id,
     uint8_t abort_reason,
@@ -71,7 +73,7 @@ void MyAbortHandler(
     Error_Detected = true;
 }
 
-void MyRejectHandler(
+static void MyRejectHandler(
     BACNET_ADDRESS * src,
     uint8_t invoke_id,
     uint8_t reject_reason)
@@ -83,7 +85,7 @@ void MyRejectHandler(
     Error_Detected = true;
 }
 
-void router_handler(
+static void My_Router_Handler(
     BACNET_ADDRESS *src,
     BACNET_NPDU_DATA *npdu_data,
     uint8_t * npdu,      /* PDU data */
@@ -169,6 +171,40 @@ void router_handler(
     }
 }
 
+static void My_NPDU_Handler(
+    BACNET_ADDRESS * src,       /* source address */
+    uint8_t * pdu,      /* PDU data */
+    uint16_t pdu_len)
+{       /* length PDU  */
+    int apdu_offset = 0;
+    BACNET_ADDRESS dest = { 0 };
+    BACNET_NPDU_DATA npdu_data = { 0 };
+
+    apdu_offset = npdu_decode(&pdu[0], &dest, src, &npdu_data);
+    if (npdu_data.network_layer_message) {
+        My_Router_Handler(src,&npdu_data,&pdu[apdu_offset],
+            (uint16_t) (pdu_len - apdu_offset));
+    } else if ((apdu_offset > 0) && (apdu_offset <= pdu_len)) {
+        if ((npdu_data.protocol_version == BACNET_PROTOCOL_VERSION) &&
+            ((dest.net == 0) || (dest.net == BACNET_BROADCAST_NETWORK))) {
+            /* only handle the version that we know how to handle */
+            /* and we are not a router, so ignore messages with
+               routing information cause they are not for us */
+            apdu_handler(src, &pdu[apdu_offset],
+                (uint16_t) (pdu_len - apdu_offset));
+        } else {
+            if (dest.net) {
+                debug_printf("NPDU: DNET=%d.  Discarded!\n", dest.net);
+            } else {
+                debug_printf("NPDU: BACnet Protocol Version=%d.  Discarded!\n",
+                    npdu_data.protocol_version);
+            }
+        }
+    }
+
+    return;
+}
+
 static void Init_Service_Handlers(
     void)
 {
@@ -379,7 +415,7 @@ int main(int argc, char *argv[]) {
         pdu_len = datalink_receive(&src, &Rx_Buf[0], MAX_MPDU, timeout);
         /* process */
         if (pdu_len) {
-            npdu_handler(&src, &Rx_Buf[0], pdu_len);
+            My_NPDU_Handler(&src, &Rx_Buf[0], pdu_len);
         }
         if (Error_Detected)
             break;