Fix the gateway example routing and lookups. Thanks, Sam! (#163)

Co-authored-by: Steve Karg <skarg@users.sourceforge.net>

src/bacnet/basic/npdu/h_routed_npdu.c

@@ -206,6 +206,7 @@ static void routed_apdu_handler(BACNET_ADDRESS *src,
         /* If wasn't unicast to us, must have been one of the bcast types.
          * Drop it. */
         if (bvlc_get_function_code() != BVLC_ORIGINAL_UNICAST_NPDU) {
+            debug_printf("NPDU: not unicast - dropped!\n");
             return;
         }
 #endif
@@ -216,7 +217,10 @@ static void routed_apdu_handler(BACNET_ADDRESS *src,
         if (dest->len > 0) {
             Send_Reject_Message_To_Network(
                 src, NETWORK_REJECT_NO_ROUTE, dest->net);
-        } /* else, silently drop it */
+        } else {
+            /* silently drop it */
+            debug_printf("NPDU: broadcast - dropped!\n");
+        }
         return;
     }
 
@@ -229,6 +233,7 @@ static void routed_apdu_handler(BACNET_ADDRESS *src,
     }
     if (!bGotOne) {
         /* Just silently drop this packet. */
+        debug_printf("NPDU: dest not found - dropped!\n");
     }
 }
 
@@ -278,6 +283,7 @@ void routing_npdu_handler(
                 network_control_handler(src, DNET_list, &npdu_data,
                     &pdu[apdu_offset], (uint16_t)(pdu_len - apdu_offset));
             } else {
+                debug_printf("NPDU: message for our router? Discarded!\n");
                 /* The DNET is set, but we don't support downstream routers,
                  * so we just silently drop this network layer message,
                  * since only routers can handle it (even if for our DNET) */

src/bacnet/basic/object/gateway/gw_device.c

@@ -220,24 +220,24 @@ void routed_get_my_address(BACNET_ADDRESS *my_address)
  *         Else False if no match or invalid idx is given.
  */
 bool Routed_Device_Address_Lookup(
-    int idx, uint8_t address_len, uint8_t *mac_adress)
+    int idx, uint8_t dlen, uint8_t *dadr)
 {
     bool result = false;
     DEVICE_OBJECT_DATA *pDev = &Devices[idx];
     int i;
 
     if ((idx >= 0) && (idx < MAX_NUM_DEVICES)) {
-        if (address_len == 0) {
+        if (dlen == 0) {
             /* Automatic match */
             iCurrent_Device_Idx = idx;
             result = true;
-        } else if (mac_adress != NULL) {
-            for (i = 0; i < address_len; i++) {
-                if (pDev->bacDevAddr.mac[i] != mac_adress[i]) {
+        } else if (dadr != NULL) {
+            for (i = 0; i < dlen; i++) {
+                if (pDev->bacDevAddr.adr[i] != dadr[i]) {
                     break;
                 }
             }
-            if (i == address_len) { /* Success! */
+            if (i == dlen) { /* Success! */
                 iCurrent_Device_Idx = idx;
                 result = true;
             }
@@ -370,25 +370,51 @@ uint32_t Routed_Device_Index_To_Instance(unsigned index)
     return Devices[iCurrent_Device_Idx].bacObj.Object_Instance_Number;
 }
 
-/** See if the requested Object instance matches that for the currently
- * indexed Device Object.
- * iCurrent_Device_Idx must have been set to point to this Device Object
- * before this function is called.
- * @param object_id [in] Object ID of the desired Device object.
- * 			If the wildcard value (BACNET_MAX_INSTANCE), always
- * matches.
- * @return True if Object ID matches the present Device, else False.
+/**
+ * For a given object instance-number, determines a 1..N-1 index
+ * of Device objects where N is MAX_NUM_DEVICES
+ *
+ * @param  object_instance - object-instance number of the object
+ * @return  index for the given instance-number, or 0 if not valid.
+ */
+static uint32_t Routed_Device_Instance_To_Index(
+        uint32_t Instance_Number)
+{
+    int i;
+
+
+    for ( i=0; i < MAX_NUM_DEVICES; i++) {
+        if (Devices[i].bacObj.Object_Instance_Number == Instance_Number)
+        {
+            /* Found Instance, so return the Device Index Number */
+            return i;
+        }
+    }
+
+    /* We did not find instance... so simply return an Index of 0
+       All gateways will have at least a single root Device Object */
+    return 0;
+
+}
+
+/**
+ * Determines if a given Device instance is valid
+ *
+ * @param  object_id - object-instance number of the object
+ * @return  true if the instance is valid, and false if not
  */
 bool Routed_Device_Valid_Object_Instance_Number(uint32_t object_id)
 {
-    bool bResult = false;
-    DEVICE_OBJECT_DATA *pDev = &Devices[iCurrent_Device_Idx];
+    bool valid = false;
+    DEVICE_OBJECT_DATA *pDev = NULL;
 
+    iCurrent_Device_Idx = Routed_Device_Instance_To_Index(object_id);
+    pDev = &Devices[iCurrent_Device_Idx];
     if (pDev->bacObj.Object_Instance_Number == object_id) {
-        bResult = true;
+        valid = true;
     }
 
-    return bResult;
+    return valid;
 }
 
 bool Routed_Device_Name(