diff --git a/bacnet-stack/demo/handler/h_alarm_ack.c b/bacnet-stack/demo/handler/h_alarm_ack.c
index 85571a37..17846296 100644
--- a/bacnet-stack/demo/handler/h_alarm_ack.c
+++ b/bacnet-stack/demo/handler/h_alarm_ack.c
@@ -69,11 +69,7 @@ void handler_alarm_ack(
     BACNET_ADDRESS my_address;
 
     /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     pdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
diff --git a/bacnet-stack/demo/handler/h_arf.c b/bacnet-stack/demo/handler/h_arf.c
index ef8ad3b4..36278bef 100644
--- a/bacnet-stack/demo/handler/h_arf.c
+++ b/bacnet-stack/demo/handler/h_arf.c
@@ -118,11 +118,7 @@ void handler_atomic_read_file(
     fprintf(stderr, "Received Atomic-Read-File Request!\n");
 #endif
     /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     pdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
@@ -174,7 +170,7 @@ void handler_atomic_read_file(
                     service_data->invoke_id,
                     ABORT_REASON_SEGMENTATION_NOT_SUPPORTED, true);
 #if PRINT_ENABLED
-                fprintf(stderr, "Too Big To Send (%d >= %d). Sending Abort!\n",
+                fprintf(stderr, "Too Big To Send (%d >= %zu). Sending Abort!\n",
                     data.type.stream.requestedOctetCount,
                     octetstring_capacity(&data.fileData));
 #endif
diff --git a/bacnet-stack/demo/handler/h_awf.c b/bacnet-stack/demo/handler/h_awf.c
index afcd5901..156bae0a 100644
--- a/bacnet-stack/demo/handler/h_awf.c
+++ b/bacnet-stack/demo/handler/h_awf.c
@@ -96,11 +96,7 @@ void handler_atomic_write_file(
     fprintf(stderr, "Received AtomicWriteFile Request!\n");
 #endif
     /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     pdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
@@ -132,7 +128,7 @@ void handler_atomic_write_file(
         } else if (data.access == FILE_STREAM_ACCESS) {
             if (bacfile_write_stream_data(&data)) {
 #if PRINT_ENABLED
-                fprintf(stderr, "AWF: Stream offset %d, %d bytes\n",
+                fprintf(stderr, "AWF: Stream offset %d, %zu bytes\n",
                     data.type.stream.fileStartPosition,
                     octetstring_length(&data.fileData));
 #endif
diff --git a/bacnet-stack/demo/handler/h_cov.c b/bacnet-stack/demo/handler/h_cov.c
index b3436a59..c1eac71d 100644
--- a/bacnet-stack/demo/handler/h_cov.c
+++ b/bacnet-stack/demo/handler/h_cov.c
@@ -326,11 +326,7 @@ static bool cov_send_request(
 #if PRINT_ENABLED
     fprintf(stderr, "COVnotification: requested\n");
 #endif
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     pdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], &cov_subscription->dest,
@@ -528,11 +524,7 @@ void handler_cov_subscribe(
     /* initialize a common abort code */
     cov_data.error_code = ABORT_REASON_SEGMENTATION_NOT_SUPPORTED;
     /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     npdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
diff --git a/bacnet-stack/demo/handler/h_dcc.c b/bacnet-stack/demo/handler/h_dcc.c
index 1e69fd7d..503c969b 100644
--- a/bacnet-stack/demo/handler/h_dcc.c
+++ b/bacnet-stack/demo/handler/h_dcc.c
@@ -103,11 +103,7 @@ void handler_device_communication_control(
     BACNET_ADDRESS my_address;
 
     /* encode the NPDU portion of the reply packet */
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     pdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
diff --git a/bacnet-stack/demo/handler/h_getevent.c b/bacnet-stack/demo/handler/h_getevent.c
index b6641174..f5499e2c 100644
--- a/bacnet-stack/demo/handler/h_getevent.c
+++ b/bacnet-stack/demo/handler/h_getevent.c
@@ -72,11 +72,7 @@ void handler_get_event_information(
     int valid_event = 0;
 
     /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     pdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
diff --git a/bacnet-stack/demo/handler/h_lso.c b/bacnet-stack/demo/handler/h_lso.c
index 19d6f6b9..196497f3 100644
--- a/bacnet-stack/demo/handler/h_lso.c
+++ b/bacnet-stack/demo/handler/h_lso.c
@@ -54,11 +54,7 @@ void handler_lso(
     BACNET_ADDRESS my_address;
 
     /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     pdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
diff --git a/bacnet-stack/demo/handler/h_npdu.c b/bacnet-stack/demo/handler/h_npdu.c
index 7bb2eaad..10019a16 100644
--- a/bacnet-stack/demo/handler/h_npdu.c
+++ b/bacnet-stack/demo/handler/h_npdu.c
@@ -46,6 +46,8 @@
  *  and passing the remaining bytes to the apdu_handler.
  *  @note The routing (except src) and NCPI information, including 
  *  npdu_data->data_expecting_reply, are discarded.
+ * @see routing_npdu_handler
+ *  
  * @ingroup MISCHNDLR
  *  
  * @param src  [out] Returned with routing source information if the NPDU 
diff --git a/bacnet-stack/demo/handler/h_pt.c b/bacnet-stack/demo/handler/h_pt.c
index fa3afc79..5051104d 100644
--- a/bacnet-stack/demo/handler/h_pt.c
+++ b/bacnet-stack/demo/handler/h_pt.c
@@ -231,11 +231,7 @@ void handler_conf_private_trans(
     /* encode the NPDU portion of the response packet as it will be needed */
     /* no matter what the outcome. */
 
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     pdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
diff --git a/bacnet-stack/demo/handler/h_rd.c b/bacnet-stack/demo/handler/h_rd.c
index 560fd610..3c1d048a 100644
--- a/bacnet-stack/demo/handler/h_rd.c
+++ b/bacnet-stack/demo/handler/h_rd.c
@@ -77,11 +77,7 @@ void handler_reinitialize_device(
     BACNET_ADDRESS my_address;
 
     /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     pdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
diff --git a/bacnet-stack/demo/handler/h_routed_npdu.c b/bacnet-stack/demo/handler/h_routed_npdu.c
index 8d8e652a..a7440d32 100644
--- a/bacnet-stack/demo/handler/h_routed_npdu.c
+++ b/bacnet-stack/demo/handler/h_routed_npdu.c
@@ -197,30 +197,16 @@ static void routed_apdu_handler(
     uint8_t * apdu,      
     uint16_t apdu_len)
 {       
-    int dnet = DNET_list[0];	/* Get the DNET of our virtual network */
-    int i;
+    int cursor = 0;				/* Starting hint */
+    bool bGotOne = false;
     
-    /* First, see if it's for the main Gateway Device, either because 
-     * there's no routing info or else its a BACnet broadcast.
-     */
-    if ( (dest->net == 0 ) || (dest->net == BACNET_BROADCAST_NETWORK)) {
-        /* Handle like a normal, non-routed access of the Gateway Device.
-         * But first, make sure our internal access is pointing at
-         * that Device in our table by telling it "no routing info" : */
-        Lookup_Routed_Device_Address( 0, 0, NULL );
-        apdu_handler(src, apdu, apdu_len);
+   	while ( Routed_Device_GetNext( dest, DNET_list, &cursor ) ) {
+			apdu_handler(src, apdu, apdu_len);
+			bGotOne = true;
+			if ( cursor < 0 )	/* If no more matches, */
+				break;			/* We don't need to keep looking */
     }
-    /* Now check for our virtual DNET or BACnet broadcast, and check
-     * against each of our virtually routed Devices.
-     * If we get a match, have it handle the APDU.
-     * For broadcasts, all Devices get a chance at it.
-     */
-    if ((dest->net == dnet) || (dest->net == BACNET_BROADCAST_NETWORK)) {
-    	for ( i = 1; i < MAX_NUM_DEVICES; i++ ) {
-            if ( Lookup_Routed_Device_Address( i, dest->len, dest->adr ) )
-            	apdu_handler(src, apdu, apdu_len);
-    	}
-    } else if ( dest->net != 0 )  {
+    if ( !bGotOne )  {
         /* We don't know how to reach this one */
         Send_Reject_Message_To_Network( src, NETWORK_REJECT_NO_ROUTE, dest->net );
     }
@@ -237,6 +223,7 @@ static void routed_apdu_handler(
  *      destination.
  *  - Errors in decoding.
  * @note The npdu_data->data_expecting_reply status is discarded.
+ * @see npdu_handler
  * @ingroup NMRC
  *  
  * @param src  [out] Returned with routing source information if the NPDU 
diff --git a/bacnet-stack/demo/handler/h_rp.c b/bacnet-stack/demo/handler/h_rp.c
index 6c09e539..aedbe198 100644
--- a/bacnet-stack/demo/handler/h_rp.c
+++ b/bacnet-stack/demo/handler/h_rp.c
@@ -83,11 +83,7 @@ void handler_read_property(
     /* configure default error code as an abort since it is common */
     rpdata.error_code = ERROR_CODE_ABORT_SEGMENTATION_NOT_SUPPORTED;
     /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     npdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
diff --git a/bacnet-stack/demo/handler/h_rpm.c b/bacnet-stack/demo/handler/h_rpm.c
index a5b587d9..f23fa0d4 100644
--- a/bacnet-stack/demo/handler/h_rpm.c
+++ b/bacnet-stack/demo/handler/h_rpm.c
@@ -204,11 +204,7 @@ void handler_read_property_multiple(
     /* jps_debug - see if we are utilizing all the buffer */
     /* memset(&Handler_Transmit_Buffer[0], 0xff, sizeof(Handler_Transmit_Buffer)); */
     /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     npdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
diff --git a/bacnet-stack/demo/handler/h_rr.c b/bacnet-stack/demo/handler/h_rr.c
index fa2bcb6e..45d01311 100644
--- a/bacnet-stack/demo/handler/h_rr.c
+++ b/bacnet-stack/demo/handler/h_rr.c
@@ -104,11 +104,7 @@ void handler_read_range(
     data.error_class = ERROR_CLASS_OBJECT;
     data.error_code = ERROR_CODE_UNKNOWN_OBJECT;
     /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     pdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
diff --git a/bacnet-stack/demo/handler/h_wp.c b/bacnet-stack/demo/handler/h_wp.c
index ae505bd3..b1ec68c8 100644
--- a/bacnet-stack/demo/handler/h_wp.c
+++ b/bacnet-stack/demo/handler/h_wp.c
@@ -75,11 +75,7 @@ void handler_write_property(
     BACNET_ADDRESS my_address;
 
     /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     pdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
diff --git a/bacnet-stack/demo/handler/noserv.c b/bacnet-stack/demo/handler/noserv.c
index c32ea245..0781e98a 100644
--- a/bacnet-stack/demo/handler/noserv.c
+++ b/bacnet-stack/demo/handler/noserv.c
@@ -65,11 +65,7 @@ void handler_unrecognized_service(
     (void) service_len;
 
     /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     npdu_encode_npdu_data(&npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     pdu_len =
         npdu_encode_pdu(&Handler_Transmit_Buffer[0], src, &my_address,
diff --git a/bacnet-stack/demo/handler/s_ack_alarm.c b/bacnet-stack/demo/handler/s_ack_alarm.c
index e01712db..2835deff 100644
--- a/bacnet-stack/demo/handler/s_ack_alarm.c
+++ b/bacnet-stack/demo/handler/s_ack_alarm.c
@@ -73,11 +73,7 @@ uint8_t Send_Alarm_Acknowledgement(
         invoke_id = tsm_next_free_invokeID();
     if (invoke_id) {
         /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-        my_address = *Get_Routed_Device_Address(-1);
-#else
         datalink_get_my_address(&my_address);
-#endif
         npdu_encode_npdu_data(&npdu_data, true, MESSAGE_PRIORITY_NORMAL);
         pdu_len =
             npdu_encode_pdu(&Handler_Transmit_Buffer[0], &dest, &my_address,
diff --git a/bacnet-stack/demo/handler/s_arfs.c b/bacnet-stack/demo/handler/s_arfs.c
index 68a8ae51..15d43971 100644
--- a/bacnet-stack/demo/handler/s_arfs.c
+++ b/bacnet-stack/demo/handler/s_arfs.c
@@ -79,11 +79,7 @@ uint8_t Send_Atomic_Read_File_Stream(
         data.type.stream.fileStartPosition = fileStartPosition;
         data.type.stream.requestedOctetCount = requestedOctetCount;
         /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-        my_address = *Get_Routed_Device_Address(-1);
-#else
         datalink_get_my_address(&my_address);
-#endif
         npdu_encode_npdu_data(&npdu_data, true, MESSAGE_PRIORITY_NORMAL);
         pdu_len =
             npdu_encode_pdu(&Handler_Transmit_Buffer[0], &dest, &my_address,
diff --git a/bacnet-stack/demo/handler/s_awfs.c b/bacnet-stack/demo/handler/s_awfs.c
index 507093f7..b0aef8bb 100644
--- a/bacnet-stack/demo/handler/s_awfs.c
+++ b/bacnet-stack/demo/handler/s_awfs.c
@@ -80,11 +80,7 @@ uint8_t Send_Atomic_Write_File_Stream(
         status = octetstring_copy(&data.fileData, fileData);
         if (status) {
             /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-            my_address = *Get_Routed_Device_Address(-1);
-#else
             datalink_get_my_address(&my_address);
-#endif
             npdu_encode_npdu_data(&npdu_data, true, MESSAGE_PRIORITY_NORMAL);
             pdu_len =
                 npdu_encode_pdu(&Handler_Transmit_Buffer[0], &dest,
diff --git a/bacnet-stack/demo/handler/s_cevent.c b/bacnet-stack/demo/handler/s_cevent.c
index 709e31e9..0c362490 100644
--- a/bacnet-stack/demo/handler/s_cevent.c
+++ b/bacnet-stack/demo/handler/s_cevent.c
@@ -70,11 +70,7 @@ uint8_t Send_CEvent_Notify(
         invoke_id = tsm_next_free_invokeID();
     if (invoke_id) {
         /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-	    my_address = *Get_Routed_Device_Address(-1);
-#else
     	datalink_get_my_address(&my_address);
-#endif
         npdu_encode_npdu_data(&npdu_data, true, MESSAGE_PRIORITY_NORMAL);
         pdu_len =
             npdu_encode_pdu(&Handler_Transmit_Buffer[0], &dest, &my_address,
diff --git a/bacnet-stack/demo/handler/s_cov.c b/bacnet-stack/demo/handler/s_cov.c
index e717d4fe..4a7b01d0 100644
--- a/bacnet-stack/demo/handler/s_cov.c
+++ b/bacnet-stack/demo/handler/s_cov.c
@@ -127,11 +127,7 @@ uint8_t Send_COV_Subscribe(
     }
     if (invoke_id) {
         /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-        my_address = *Get_Routed_Device_Address(-1);
-#else
         datalink_get_my_address(&my_address);
-#endif
         npdu_encode_npdu_data(&npdu_data, true, MESSAGE_PRIORITY_NORMAL);
         pdu_len =
             npdu_encode_pdu(&Handler_Transmit_Buffer[0], &dest, &my_address,
diff --git a/bacnet-stack/demo/handler/s_dcc.c b/bacnet-stack/demo/handler/s_dcc.c
index 5d2f069c..1f4653f7 100644
--- a/bacnet-stack/demo/handler/s_dcc.c
+++ b/bacnet-stack/demo/handler/s_dcc.c
@@ -83,11 +83,7 @@ uint8_t Send_Device_Communication_Control_Request(
         invoke_id = tsm_next_free_invokeID();
     if (invoke_id) {
         /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-        my_address = *Get_Routed_Device_Address(-1);
-#else
         datalink_get_my_address(&my_address);
-#endif
         npdu_encode_npdu_data(&npdu_data, true, MESSAGE_PRIORITY_NORMAL);
         pdu_len =
             npdu_encode_pdu(&Handler_Transmit_Buffer[0], &dest, &my_address,
diff --git a/bacnet-stack/demo/handler/s_iam.c b/bacnet-stack/demo/handler/s_iam.c
index 14ce2e68..bc29f5fe 100644
--- a/bacnet-stack/demo/handler/s_iam.c
+++ b/bacnet-stack/demo/handler/s_iam.c
@@ -141,11 +141,7 @@ int iam_unicast_encode_pdu(
     memcpy(dest, src, sizeof(BACNET_ADDRESS));
     dest->net = 0;
 
-#if BAC_ROUTING
-    my_address = *Get_Routed_Device_Address(-1);
-#else
     datalink_get_my_address(&my_address);
-#endif
     /* encode the NPDU portion of the packet */
     npdu_encode_npdu_data(npdu_data, false, MESSAGE_PRIORITY_NORMAL);
     npdu_len = npdu_encode_pdu(&buffer[0], dest, &my_address, npdu_data);
diff --git a/bacnet-stack/demo/handler/s_lso.c b/bacnet-stack/demo/handler/s_lso.c
index e7207a2c..bc518144 100644
--- a/bacnet-stack/demo/handler/s_lso.c
+++ b/bacnet-stack/demo/handler/s_lso.c
@@ -73,11 +73,7 @@ uint8_t Send_Life_Safety_Operation_Data(
         invoke_id = tsm_next_free_invokeID();
     if (invoke_id) {
         /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-        my_address = *Get_Routed_Device_Address(-1);
-#else
         datalink_get_my_address(&my_address);
-#endif
         npdu_encode_npdu_data(&npdu_data, true, MESSAGE_PRIORITY_NORMAL);
         pdu_len =
             npdu_encode_pdu(&Handler_Transmit_Buffer[0], &dest, &my_address,
diff --git a/bacnet-stack/demo/handler/s_ptransfer.c b/bacnet-stack/demo/handler/s_ptransfer.c
index 747cfa58..577739b6 100644
--- a/bacnet-stack/demo/handler/s_ptransfer.c
+++ b/bacnet-stack/demo/handler/s_ptransfer.c
@@ -78,11 +78,7 @@ uint8_t Send_Private_Transfer_Request(
         invoke_id = tsm_next_free_invokeID();
     if (invoke_id) {
         /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-        my_address = *Get_Routed_Device_Address(-1);
-#else
         datalink_get_my_address(&my_address);
-#endif
         npdu_encode_npdu_data(&npdu_data, true, MESSAGE_PRIORITY_NORMAL);
         pdu_len =
             npdu_encode_pdu(&Handler_Transmit_Buffer[0], &dest, &my_address,
diff --git a/bacnet-stack/demo/handler/s_rd.c b/bacnet-stack/demo/handler/s_rd.c
index 0c61d3f8..f2fdc5b0 100644
--- a/bacnet-stack/demo/handler/s_rd.c
+++ b/bacnet-stack/demo/handler/s_rd.c
@@ -80,11 +80,7 @@ uint8_t Send_Reinitialize_Device_Request(
         invoke_id = tsm_next_free_invokeID();
     if (invoke_id) {
         /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-        my_address = *Get_Routed_Device_Address(-1);
-#else
         datalink_get_my_address(&my_address);
-#endif
         npdu_encode_npdu_data(&npdu_data, true, MESSAGE_PRIORITY_NORMAL);
         pdu_len =
             npdu_encode_pdu(&Handler_Transmit_Buffer[0], &dest, &my_address,
diff --git a/bacnet-stack/demo/handler/s_readrange.c b/bacnet-stack/demo/handler/s_readrange.c
index 8fe3f3c2..e342bb78 100644
--- a/bacnet-stack/demo/handler/s_readrange.c
+++ b/bacnet-stack/demo/handler/s_readrange.c
@@ -72,11 +72,7 @@ uint8_t Send_ReadRange_Request(
 
     if (invoke_id) {
         /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-        my_address = *Get_Routed_Device_Address(-1);
-#else
         datalink_get_my_address(&my_address);
-#endif
         npdu_encode_npdu_data(&npdu_data, true, MESSAGE_PRIORITY_NORMAL);
         pdu_len =
             npdu_encode_pdu(&Handler_Transmit_Buffer[0], &dest, &my_address,
diff --git a/bacnet-stack/demo/handler/s_rp.c b/bacnet-stack/demo/handler/s_rp.c
index 511f4800..240f16d3 100644
--- a/bacnet-stack/demo/handler/s_rp.c
+++ b/bacnet-stack/demo/handler/s_rp.c
@@ -86,11 +86,7 @@ uint8_t Send_Read_Property_Request(
         invoke_id = tsm_next_free_invokeID();
     if (invoke_id) {
         /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-        my_address = *Get_Routed_Device_Address(-1);
-#else
         datalink_get_my_address(&my_address);
-#endif
         npdu_encode_npdu_data(&npdu_data, true, MESSAGE_PRIORITY_NORMAL);
         pdu_len =
             npdu_encode_pdu(&Handler_Transmit_Buffer[0], &dest, &my_address,
diff --git a/bacnet-stack/demo/handler/s_rpm.c b/bacnet-stack/demo/handler/s_rpm.c
index 0b6fd9d2..25700f96 100644
--- a/bacnet-stack/demo/handler/s_rpm.c
+++ b/bacnet-stack/demo/handler/s_rpm.c
@@ -81,11 +81,7 @@ uint8_t Send_Read_Property_Multiple_Request(
         invoke_id = tsm_next_free_invokeID();
     if (invoke_id) {
         /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-        my_address = *Get_Routed_Device_Address(-1);
-#else
         datalink_get_my_address(&my_address);
-#endif
         npdu_encode_npdu_data(&npdu_data, true, MESSAGE_PRIORITY_NORMAL);
         pdu_len = npdu_encode_pdu(&pdu[0], &dest, &my_address, &npdu_data);
         /* encode the APDU portion of the packet */
diff --git a/bacnet-stack/demo/handler/s_wp.c b/bacnet-stack/demo/handler/s_wp.c
index 77459c57..d6e46192 100644
--- a/bacnet-stack/demo/handler/s_wp.c
+++ b/bacnet-stack/demo/handler/s_wp.c
@@ -77,11 +77,7 @@ uint8_t Send_Write_Property_Request_Data(
         invoke_id = tsm_next_free_invokeID();
     if (invoke_id) {
         /* encode the NPDU portion of the packet */
-#if BAC_ROUTING
-        my_address = *Get_Routed_Device_Address(-1);
-#else
         datalink_get_my_address(&my_address);
-#endif
         npdu_encode_npdu_data(&npdu_data, true, MESSAGE_PRIORITY_NORMAL);
         pdu_len =
             npdu_encode_pdu(&Handler_Transmit_Buffer[0], &dest, &my_address,
diff --git a/bacnet-stack/demo/object/gw_device.c b/bacnet-stack/demo/object/gw_device.c
index a5975432..cb37bb4c 100644
--- a/bacnet-stack/demo/object/gw_device.c
+++ b/bacnet-stack/demo/object/gw_device.c
@@ -156,7 +156,7 @@ DEVICE_OBJECT_DATA * Get_Routed_Device_Object(
 		return NULL;
 }
 
-/** Return the BACnet addreess for the indicated entry.
+/** Return the BACnet address for the indicated entry.
  * @param idx [in] Index into Devices[] array being requested.
  *                 0 is for the main, gateway Device entry.
  *                 -1 is a special case meaning "whichever iCurrent_Device_Idx
@@ -175,6 +175,21 @@ BACNET_ADDRESS * Get_Routed_Device_Address(
 		return NULL;
 }
 
+/** Get the currently active BACnet address.
+ * This is an implementation of the datalink_get_my_address() template for 
+ * devices with routing.
+ * 
+ * @param my_address [out] Points to the currently active Device Object's
+ * 							BACnet address.
+ */
+void routed_get_my_address(
+    BACNET_ADDRESS * my_address)
+
+{
+	my_address = &Devices[iCurrent_Device_Idx].bacDevAddr;
+}
+
+
 /** See if the Gateway or Routed Device at the given idx matches
  * the given MAC address.
  * Has the desirable side-effect of setting iCurrent_Device_Idx to the
@@ -192,7 +207,7 @@ BACNET_ADDRESS * Get_Routed_Device_Address(
  *         meaning MAC broadcast, so it's an automatic match). 
  *         Else False if no match or invalid idx is given.
  */
-bool Lookup_Routed_Device_Address( 
+bool Routed_Device_Address_Lookup( 
 		int idx, 
 		uint8_t address_len,       
 		uint8_t * mac_adress )
@@ -206,7 +221,7 @@ bool Lookup_Routed_Device_Address(
 			/* Automatic match */
 			iCurrent_Device_Idx = idx;
 			result = true;
-		} else {
+		} else if ( mac_adress != NULL ) {
 		    for (i = 0; i < address_len; i++) {
 		        if (pDev->bacDevAddr.mac[i] != mac_adress[i])
 		            break;
@@ -220,6 +235,88 @@ bool Lookup_Routed_Device_Address(
 	return result;
 }
 
+
+/** Find the next Gateway or Routed Device at the given MAC address, 
+ * starting the search at the "cursor".
+ * Has the desirable side-effect of setting internal iCurrent_Device_Idx 
+ * if a match is found, for use in the subsequent routing handling 
+ * functions.
+ * 
+ * @param dest [in] The BACNET_ADDRESS of the message's destination.
+ * 		   If the Length of the mac_adress[] field is 0, then this is a MAC 
+ * 		   broadcast.  Otherwise, size is determined
+ *         by the DLL type (eg, 6 for BIP and 2 for MSTP).
+ * @param DNET_list [in] List of our reachable downstream BACnet Network numbers.
+ * 					 Normally just one valid entry; terminated with a -1 value.
+ * @param cursor [in,out] The concept of the cursor is that it is a starting
+ * 		   "hint" for the search; on return, it is updated to provide the
+ * 		   cursor value to use with a subsequent GetNext call, or it 
+ *         equals -1 if there are no further matches.
+ *         Set it to 0 on entry to access the main, gateway Device entry, or
+ *         to start looping through the routed devices.
+ *         Otherwise, its returned value is implementation-dependent and the 
+ *         calling function should not alter or interpret it.
+ * 
+ * @return True if the MAC addresses match (or the address_len is 0, 
+ *         meaning MAC broadcast, so it's an automatic match). 
+ *         Else False if no match or invalid idx is given; the cursor will
+ *         be returned as -1 in these cases.
+ */
+bool Routed_Device_GetNext( 
+	    BACNET_ADDRESS * dest,
+	    int * DNET_list,
+		int * cursor )
+{
+    int dnet = DNET_list[0];	/* Get the DNET of our virtual network */
+    int idx = *cursor;
+    bool bSuccess = false;
+    
+    /* First, see if it's a BACnet broadcast. 
+     * For broadcasts, all Devices get a chance at it.
+     */
+    if (dest->net == BACNET_BROADCAST_NETWORK) {
+    	/* Just take the entry indexed by the cursor */
+    	bSuccess = Routed_Device_Address_Lookup( idx++, 
+    					dest->len, dest->adr );
+    }
+    /* Or see if it's for the main Gateway Device, because 
+     * there's no routing info.
+     */
+    else if (dest->net == 0)  {
+        /* Handle like a normal, non-routed access of the Gateway Device.
+         * But first, make sure our internal access is pointing at
+         * that Device in our table by telling it "no routing info" : */
+    	bSuccess = Routed_Device_Address_Lookup( 0, 
+    					dest->len, dest->adr );
+    	/* Next step: no more matches: */
+    	idx = -1;
+    }
+    /* Or if is our virtual DNET, check
+     * against each of our virtually routed Devices.
+     * If we get a match, have it handle the APDU.
+     * For broadcasts, all Devices get a chance at it.
+     */
+    else if (dest->net == dnet)  {
+    	if ( idx == 0 )		/* Step over this case (starting point) */
+    		idx = 1;
+    	while (idx < MAX_NUM_DEVICES) {
+        	bSuccess = Routed_Device_Address_Lookup( idx++, 
+        					dest->len, dest->adr );
+        	if ( bSuccess )
+				break;			/* We don't need to keep looking */
+    	}
+    } 
+
+    if ( !bSuccess )
+    	*cursor = -1;
+    else if ( idx == MAX_NUM_DEVICES )		/* No more to GetNext */
+    	*cursor = -1;
+    else
+    	*cursor = idx;
+    return bSuccess;
+}
+
+
 /* methods to override the normal Device objection functions */
 
 uint32_t Routed_Device_Index_To_Instance(
diff --git a/bacnet-stack/demo/readfile/main.c b/bacnet-stack/demo/readfile/main.c
index e916f9d4..4c27f4b7 100644
--- a/bacnet-stack/demo/readfile/main.c
+++ b/bacnet-stack/demo/readfile/main.c
@@ -144,7 +144,7 @@ static void AtomicReadFileAckHandler(
                 } else if (octets_written == 0) {
                     fprintf(stderr, "Received 0 byte octet string!.\n");
                 } else {
-                    printf("\r%u bytes",
+                    printf("\r%zu bytes",
                         (data.type.stream.fileStartPosition + octets_written));
                 }
                 fflush(pFile);
diff --git a/bacnet-stack/demo/writefile/main.c b/bacnet-stack/demo/writefile/main.c
index fd2c69c2..8e70676c 100644
--- a/bacnet-stack/demo/writefile/main.c
+++ b/bacnet-stack/demo/writefile/main.c
@@ -279,7 +279,7 @@ int main(
                     fclose(pFile);
                 } else
                     End_Of_File_Detected = true;
-                printf("\rSending %d bytes", (fileStartPosition + len));
+                printf("\rSending %zu bytes", (fileStartPosition + len));
                 invoke_id =
                     Send_Atomic_Write_File_Stream
                     (Target_Device_Object_Instance,
diff --git a/bacnet-stack/include/bacapp.h b/bacnet-stack/include/bacapp.h
index 908450e4..8fdfea56 100644
--- a/bacnet-stack/include/bacapp.h
+++ b/bacnet-stack/include/bacapp.h
@@ -197,10 +197,8 @@ extern "C" {
         unsigned max_apdu_len,
         BACNET_PROPERTY_ID property);
 
-#if PRINT_ENABLED
-#define BACAPP_PRINT_ENABLED
-#else
-#ifdef TEST
+#ifndef BACAPP_PRINT_ENABLED    
+#if PRINT_ENABLED || defined TEST
 #define BACAPP_PRINT_ENABLED
 #endif
 #endif
diff --git a/bacnet-stack/include/config.h b/bacnet-stack/include/config.h
index 03fb1ead..b23c719c 100644
--- a/bacnet-stack/include/config.h
+++ b/bacnet-stack/include/config.h
@@ -47,7 +47,7 @@
 
 /* Enable the Gateway (Routing) functionality here, if desired. */
 #if !defined(BAC_ROUTING)
-#define BAC_ROUTING 0
+#define BAC_ROUTING 1
 #endif
 #if !defined(MAX_NUM_DEVICES)
 #if BAC_ROUTING
diff --git a/bacnet-stack/include/datalink.h b/bacnet-stack/include/datalink.h
index e3b41d60..5010fca5 100644
--- a/bacnet-stack/include/datalink.h
+++ b/bacnet-stack/include/datalink.h
@@ -81,7 +81,11 @@
 #endif
 #define datalink_cleanup bip_cleanup
 #define datalink_get_broadcast_address bip_get_broadcast_address
+#if BAC_ROUTING
+#define datalink_get_my_address routed_get_my_address
+#else
 #define datalink_get_my_address bip_get_my_address
+#endif
 
 #else /* Ie, BACDL_ALL */
 #include "npdu.h"
diff --git a/bacnet-stack/include/device.h b/bacnet-stack/include/device.h
index 88a1c08a..816341b1 100644
--- a/bacnet-stack/include/device.h
+++ b/bacnet-stack/include/device.h
@@ -319,10 +319,18 @@ extern "C" {
     	int idx );
     BACNET_ADDRESS * Get_Routed_Device_Address(
 		int idx );
-    bool Lookup_Routed_Device_Address( 
+    
+    void routed_get_my_address(
+        BACNET_ADDRESS * my_address);
+
+    bool Routed_Device_Address_Lookup( 
     		int idx, 
     		uint8_t address_len,        
     		uint8_t * mac_adress );
+    bool Routed_Device_GetNext( 
+    	    BACNET_ADDRESS * dest,
+    	    int * DNET_list,
+    		int * cursor );
     
     uint32_t Routed_Device_Index_To_Instance(
         unsigned index);