Add npdu set i am router to network handler (#1169)

* added npdu_set_i_am_router_to_network_handler

* npdu_send_what_is_network_number: fix sending to broadcast address

src/bacnet/basic/npdu/h_npdu.c

@@ -13,6 +13,7 @@
 #include "bacnet/bacaddr.h"
 #include "bacnet/bacdcode.h"
 #include "bacnet/bacint.h"
+#include "bacnet/bactext.h"
 #include "bacnet/npdu.h"
 #include "bacnet/apdu.h"
 #include "bacnet/basic/services.h"
@@ -26,6 +27,19 @@
 static uint16_t Local_Network_Number;
 static uint8_t Local_Network_Number_Status = NETWORK_NUMBER_LEARNED;
 
+static i_am_router_to_network_function I_Am_Router_To_Network_Function;
+
+/**
+ * @brief Set a handler function called for the I Am Router To Network message
+ *
+ * @param pFunction  Pointer to the function
+ */
+void npdu_set_i_am_router_to_network_handler(
+    i_am_router_to_network_function pFunction)
+{
+    I_Am_Router_To_Network_Function = pFunction;
+}
+
 /**
  * @brief get the local network number
  * @return local network number
@@ -109,7 +123,7 @@ int npdu_send_what_is_network_number(BACNET_ADDRESS *dst)
     pdu_len = npdu_encode_pdu(pdu, &daddr, &saddr, &npdu_data);
 
     /* Now send the message */
-    return datalink_send_pdu(dst, &npdu_data, pdu, pdu_len);
+    return datalink_send_pdu(&daddr, &npdu_data, pdu, pdu_len);
 }
 
 /** @file h_npdu.c  Handles messages at the NPDU level of the BACnet stack. */
@@ -138,6 +152,8 @@ static void network_control_handler(
 {
     uint16_t dnet = 0;
     uint8_t status = 0;
+    uint16_t npdu_offset = 0;
+    uint16_t len = 0;
 
     switch (npdu_data->network_message_type) {
         case NETWORK_MESSAGE_WHAT_IS_NETWORK_NUMBER:
@@ -177,6 +193,16 @@ static void network_control_handler(
                     that are sent with a local unicast address. */
             }
             break;
+        case NETWORK_MESSAGE_I_AM_ROUTER_TO_NETWORK:
+            if (I_Am_Router_To_Network_Function) {
+                while (npdu_len >= 2) {
+                    len = decode_unsigned16(&npdu[npdu_offset], &dnet);
+                    I_Am_Router_To_Network_Function(src, dnet);
+                    npdu_len -= len;
+                    npdu_offset += len;
+                }
+            }
+            break;
         default:
             break;
     }

src/bacnet/basic/npdu/h_npdu.h

@@ -53,6 +53,14 @@ void npdu_router_get_my_address(uint16_t dnet, BACNET_ADDRESS *my_address);
 BACNET_STACK_EXPORT
 int npdu_send_reject_message_to_network(BACNET_ADDRESS *dst, uint16_t net);
 
+/* I Am Router To Network function */
+typedef void (*i_am_router_to_network_function)(
+    BACNET_ADDRESS *src, uint16_t network);
+
+BACNET_STACK_EXPORT
+void npdu_set_i_am_router_to_network_handler(
+    i_am_router_to_network_function pFunction);
+
 #ifdef __cplusplus
 }
 #endif /* __cplusplus */