Bugfix/ipv6 leave multicast on foreign device registration (#899)

* Fixed IPv6 handler to ignore original-broadcast when registered as a foreign-device

* Fixed IPv6 to leave multicast when registering as foreign device.

ports/bsd/bip6.c

@@ -14,6 +14,7 @@
 #include "bacnet/bacdcode.h"
 #include "bacnet/config.h"
 #include "bacnet/datalink/bip6.h"
+#include "bacnet/basic/sys/debug.h"
 #include "bacnet/basic/object/device.h"
 #include "bacnet/basic/bbmd6/h_bbmd6.h"
 #if DEBUG_ENABLED
@@ -110,7 +111,7 @@ void bip6_set_interface(char *ifname)
     bool found = false;
 
     if (getifaddrs(&ifa) == -1) {
-        perror("BIP6: getifaddrs failed");
+        debug_perror("BIP6: getifaddrs failed");
         exit(1);
     }
     ifa_tmp = ifa;
@@ -396,6 +397,62 @@ void bip6_cleanup(void)
     return;
 }
 
+/**
+ * @brief Join a multicast group
+ */
+void bip6_join_group(void)
+{
+    struct in6_addr broadcast_address = { 0 };
+    struct ipv6_mreq join_request = { 0 };
+    int status = 0; /* return from socket lib calls */
+
+    if (BIP6_Socket < 0) {
+        return;
+    }
+    /* join a multicast group */
+    memcpy(
+        &broadcast_address.s6_addr[0], &BIP6_Broadcast_Addr.address[0],
+        IP6_ADDRESS_MAX);
+    memcpy(
+        &join_request.ipv6mr_multiaddr, &broadcast_address,
+        sizeof(struct in6_addr));
+    /* Let system not choose the interface */
+    join_request.ipv6mr_interface = BIP6_Socket_Scope_Id;
+    status = setsockopt(
+        BIP6_Socket, IPPROTO_IPV6, IPV6_JOIN_GROUP, &join_request,
+        sizeof(join_request));
+    if (status < 0) {
+        debug_perror("BIP6: setsockopt(IPV6_JOIN_GROUP)");
+    }
+}
+
+/**
+ * @brief Leave a multicast group
+ */
+void bip6_leave_group(void)
+{
+    struct in6_addr broadcast_address = { 0 };
+    struct ipv6_mreq leave_request = { 0 };
+    int status = 0; /* return from socket lib calls */
+
+    if (BIP6_Socket < 0) {
+        return;
+    }
+    /* leave a multicast address */
+    memcpy(
+        &broadcast_address.s6_addr[0], &BIP6_Broadcast_Addr.address[0],
+        IP6_ADDRESS_MAX);
+    memcpy(
+        &leave_request.ipv6mr_multiaddr, &broadcast_address,
+        sizeof(struct in6_addr));
+    status = setsockopt(
+        BIP6_Socket, IPPROTO_IPV6, IPV6_LEAVE_GROUP, &leave_request,
+        sizeof(leave_request));
+    if (status < 0) {
+        debug_perror("BIP6: setsockopt(IPV6_LEAVE_GROUP)");
+    }
+}
+
 /** Initialize the BACnet/IP services at the given interface.
  * @ingroup DLBIP6
  * -# Gets the local IP address and local broadcast address from the system,
@@ -417,8 +474,6 @@ bool bip6_init(char *ifname)
 {
     int status = 0; /* return from socket lib calls */
     struct sockaddr_in6 server = { 0 };
-    struct in6_addr broadcast_address;
-    struct ipv6_mreq join_request;
     int sockopt = 0;
 
     if (ifname) {
@@ -456,19 +511,7 @@ bool bip6_init(char *ifname)
         BIP6_Socket = -1;
         return false;
     }
-    /* subscribe to a multicast address */
+    bip6_join_group();
-    memcpy(&broadcast_address.s6_addr[0], &BIP6_Broadcast_Addr.address[0],
-        IP6_ADDRESS_MAX);
-    memcpy(&join_request.ipv6mr_multiaddr, &broadcast_address,
-        sizeof(struct in6_addr));
-    /* Let system not choose the interface */
-    join_request.ipv6mr_interface = BIP6_Socket_Scope_Id;
-    status = setsockopt(BIP6_Socket, IPPROTO_IPV6, IPV6_JOIN_GROUP,
-        &join_request, sizeof(join_request));
-    if (status < 0) {
-        perror("BIP: setsockopt(IPV6_JOIN_GROUP)");
-    }
-
     /* bind the socket to the local port number and IP address */
     server.sin6_family = AF_INET6;
     server.sin6_addr = in6addr_any;
@@ -476,7 +519,7 @@ bool bip6_init(char *ifname)
     debug_print_ipv6("Binding->", &server.sin6_addr);
     status = bind(BIP6_Socket, (const void *)&server, sizeof(server));
     if (status < 0) {
-        perror("BIP: bind");
+        debug_perror("BIP6: bind");
         close(BIP6_Socket);
         BIP6_Socket = -1;
         return false;

ports/linux/bip6.c

@@ -14,6 +14,7 @@
 #include "bacnet/bacdcode.h"
 #include "bacnet/config.h"
 #include "bacnet/datalink/bip6.h"
+#include "bacnet/basic/sys/debug.h"
 #include "bacnet/basic/object/device.h"
 #include "bacnet/basic/bbmd6/h_bbmd6.h"
 #if DEBUG_ENABLED
@@ -98,7 +99,7 @@ void bip6_set_interface(char *ifname)
     bool found = false;
 
     if (getifaddrs(&ifa) == -1) {
-        perror("BIP6: getifaddrs failed");
+        debug_perror("BIP6: getifaddrs failed");
         exit(1);
     }
     ifa_tmp = ifa;
@@ -391,6 +392,62 @@ void bip6_cleanup(void)
     return;
 }
 
+/**
+ * @brief Join a multicast group
+ */
+void bip6_join_group(void)
+{
+    struct in6_addr broadcast_address = { 0 };
+    struct ipv6_mreq join_request = { 0 };
+    int status = 0; /* return from socket lib calls */
+
+    if (BIP6_Socket < 0) {
+        return;
+    }
+    /* join a multicast group */
+    memcpy(
+        &broadcast_address.s6_addr[0], &BIP6_Broadcast_Addr.address[0],
+        IP6_ADDRESS_MAX);
+    memcpy(
+        &join_request.ipv6mr_multiaddr, &broadcast_address,
+        sizeof(struct in6_addr));
+    /* Let system not choose the interface */
+    join_request.ipv6mr_interface = BIP6_Socket_Scope_Id;
+    status = setsockopt(
+        BIP6_Socket, IPPROTO_IPV6, IPV6_JOIN_GROUP, &join_request,
+        sizeof(join_request));
+    if (status < 0) {
+        debug_perror("BIP6: setsockopt(IPV6_JOIN_GROUP)");
+    }
+}
+
+/**
+ * @brief Leave a multicast group
+ */
+void bip6_leave_group(void)
+{
+    struct in6_addr broadcast_address = { 0 };
+    struct ipv6_mreq leave_request = { 0 };
+    int status = 0; /* return from socket lib calls */
+
+    if (BIP6_Socket < 0) {
+        return;
+    }
+    /* leave a multicast address */
+    memcpy(
+        &broadcast_address.s6_addr[0], &BIP6_Broadcast_Addr.address[0],
+        IP6_ADDRESS_MAX);
+    memcpy(
+        &leave_request.ipv6mr_multiaddr, &broadcast_address,
+        sizeof(struct in6_addr));
+    status = setsockopt(
+        BIP6_Socket, IPPROTO_IPV6, IPV6_LEAVE_GROUP, &leave_request,
+        sizeof(leave_request));
+    if (status < 0) {
+        debug_perror("BIP6: setsockopt(IPV6_LEAVE_GROUP)");
+    }
+}
+
 /** Initialize the BACnet/IP services at the given interface.
  * @ingroup DLBIP6
  * -# Gets the local IP address and local broadcast address from the system,
@@ -412,8 +469,6 @@ bool bip6_init(char *ifname)
 {
     int status = 0; /* return from socket lib calls */
     struct sockaddr_in6 server = { 0 };
-    struct in6_addr broadcast_address;
-    struct ipv6_mreq join_request;
     int sockopt = 0;
 
     if (ifname) {
@@ -453,22 +508,7 @@ bool bip6_init(char *ifname)
         BIP6_Socket = -1;
         return false;
     }
-    /* subscribe to a multicast address */
+    bip6_join_group();
-    memcpy(
-        &broadcast_address.s6_addr[0], &BIP6_Broadcast_Addr.address[0],
-        IP6_ADDRESS_MAX);
-    memcpy(
-        &join_request.ipv6mr_multiaddr, &broadcast_address,
-        sizeof(struct in6_addr));
-    /* Let system not choose the interface */
-    join_request.ipv6mr_interface = BIP6_Socket_Scope_Id;
-    status = setsockopt(
-        BIP6_Socket, IPPROTO_IPV6, IPV6_JOIN_GROUP, &join_request,
-        sizeof(join_request));
-    if (status < 0) {
-        perror("BIP: setsockopt(IPV6_JOIN_GROUP)");
-    }
-
     /* bind the socket to the local port number and IP address */
     server.sin6_family = AF_INET6;
 #if 0
@@ -490,7 +530,7 @@ bool bip6_init(char *ifname)
     debug_print_ipv6("Binding->", &server.sin6_addr);
     status = bind(BIP6_Socket, (const void *)&server, sizeof(server));
     if (status < 0) {
-        perror("BIP: bind");
+        debug_perror("BIP6: bind");
         close(BIP6_Socket);
         BIP6_Socket = -1;
         return false;

ports/win32/bip6.c

@@ -78,8 +78,6 @@ void bip6_set_interface(char *ifname)
     int i, RetVal;
     struct addrinfo Hints, *AddrInfo, *AI;
     struct sockaddr_in6 *sin;
-    struct in6_addr broadcast_address = { 0 };
-    struct ipv6_mreq join_request = { 0 };
     SOCKET ServSock[FD_SETSIZE] = { 0 };
     char port[6] = "";
     int sockopt = 0;
@@ -102,7 +100,7 @@ void bip6_set_interface(char *ifname)
     }
     RetVal = getaddrinfo(ifname, &port[0], &Hints, &AddrInfo);
     if (RetVal != 0) {
-        fprintf(
+        debug_fprintf(
             stderr, "BIP6: getaddrinfo failed with error %d: %s\n", RetVal,
             gai_strerror(RetVal));
         WSACleanup();
@@ -117,7 +115,7 @@ void bip6_set_interface(char *ifname)
     for (i = 0, AI = AddrInfo; AI != NULL; AI = AI->ai_next) {
         /* Highly unlikely, but check anyway. */
         if (i == FD_SETSIZE) {
-            fprintf(
+            debug_fprintf(
                 stderr,
                 "BIP6: getaddrinfo returned more addresses than we could "
                 "use.\n");
@@ -137,7 +135,7 @@ void bip6_set_interface(char *ifname)
         }
         BIP6_Socket = socket(AF_INET6, SOCK_DGRAM, IPPROTO_UDP);
         if (BIP6_Socket == INVALID_SOCKET) {
-            fprintf(
+            debug_fprintf(
                 stderr, "BIP6: socket() failed with error %d: %s\n",
                 WSAGetLastError(), PrintError(WSAGetLastError()));
             continue;
@@ -146,9 +144,10 @@ void bip6_set_interface(char *ifname)
         if ((AI->ai_family == AF_INET6) &&
             IN6_IS_ADDR_LINKLOCAL(&sin->sin6_addr) &&
             (sin->sin6_scope_id == 0)) {
-            fprintf(
+            debug_fprintf(
                 stderr, "BIP6: IPv6 link local addresses needs a scope ID!\n");
         }
+        BIP6_Socket_Scope_Id = if_nametoindex(ifname);
         /* Allow us to use the same socket for sending and receiving */
         /* This makes sure that the src port is correct when sending */
         sockopt = 1;
@@ -158,7 +157,7 @@ void bip6_set_interface(char *ifname)
         if (RetVal < 0) {
             closesocket(BIP6_Socket);
             BIP6_Socket = INVALID_SOCKET;
-            fprintf(
+            debug_fprintf(
                 stderr,
                 "BIP6: setsockopt(SO_REUSEADDR) failed with error %d: %s\n",
                 WSAGetLastError(), PrintError(WSAGetLastError()));
@@ -171,39 +170,20 @@ void bip6_set_interface(char *ifname)
         if (RetVal < 0) {
             closesocket(BIP6_Socket);
             BIP6_Socket = INVALID_SOCKET;
-            fprintf(
+            debug_fprintf(
                 stderr,
                 "BIP6: setsockopt(SO_BROADCAST) failed "
                 "with error %d: %s\n",
                 WSAGetLastError(), PrintError(WSAGetLastError()));
             continue;
         }
-        /* subscribe to a multicast address */
+        bip6_join_group();
-        memcpy(
-            &broadcast_address.s6_addr[0], &BIP6_Broadcast_Addr.address[0],
-            IP6_ADDRESS_MAX);
-        memcpy(
-            &join_request.ipv6mr_multiaddr, &broadcast_address,
-            sizeof(struct in6_addr));
-        /* Let system not choose the interface */
-        BIP6_Socket_Scope_Id = if_nametoindex(ifname);
-        join_request.ipv6mr_interface = BIP6_Socket_Scope_Id;
-        RetVal = setsockopt(
-            BIP6_Socket, IPPROTO_IPV6, IPV6_JOIN_GROUP, (char *)&join_request,
-            sizeof(join_request));
-        if (RetVal < 0) {
-            fprintf(
-                stderr,
-                "BIP6: setsockopt(IPV6_JOIN_GROUP) failed "
-                "with error %d: %s\n",
-                WSAGetLastError(), PrintError(WSAGetLastError()));
-        }
         /* bind() associates a local address and port combination
            with the socket just created. This is most useful when
            the application is a server that has a well-known port
            that clients know about in advance. */
         if (bind(BIP6_Socket, AI->ai_addr, AI->ai_addrlen) == SOCKET_ERROR) {
-            fprintf(
+            debug_fprintf(
                 stderr, "BIP6: bind() failed with error %d: %s\n",
                 WSAGetLastError(), PrintError(WSAGetLastError()));
             closesocket(ServSock[i]);
@@ -230,7 +210,8 @@ void bip6_set_interface(char *ifname)
     }
     freeaddrinfo(AddrInfo);
     if (BIP6_Socket == INVALID_SOCKET) {
-        fprintf(stderr, "BIP6: AF_INET6 address not found getaddrinfo()\n");
+        debug_fprintf(
+            stderr, "BIP6: AF_INET6 address not found getaddrinfo()\n");
     }
 }
 
@@ -497,6 +478,70 @@ void bip6_cleanup(void)
     return;
 }
 
+/**
+ * @brief Join a multicast group
+ */
+void bip6_join_group(void)
+{
+    struct in6_addr broadcast_address = { 0 };
+    struct ipv6_mreq join_request = { 0 };
+    int status = 0; /* return from socket lib calls */
+
+    if (BIP6_Socket == INVALID_SOCKET) {
+        return;
+    }
+    /* join a multicast group */
+    memcpy(
+        &broadcast_address.s6_addr[0], &BIP6_Broadcast_Addr.address[0],
+        IP6_ADDRESS_MAX);
+    memcpy(
+        &join_request.ipv6mr_multiaddr, &broadcast_address,
+        sizeof(struct in6_addr));
+    /* Let system not choose the interface */
+    join_request.ipv6mr_interface = BIP6_Socket_Scope_Id;
+    status = setsockopt(
+        BIP6_Socket, IPPROTO_IPV6, IPV6_JOIN_GROUP, (char *)&join_request,
+        sizeof(join_request));
+    if (status < 0) {
+        debug_fprintf(
+            stderr,
+            "BIP6: setsockopt(IPV6_JOIN_GROUP) failed "
+            "with error %d: %s\n",
+            WSAGetLastError(), PrintError(WSAGetLastError()));
+    }
+}
+
+/**
+ * @brief Leave a multicast group
+ */
+void bip6_leave_group(void)
+{
+    struct in6_addr broadcast_address = { 0 };
+    struct ipv6_mreq leave_request = { 0 };
+    int status = 0; /* return from socket lib calls */
+
+    if (BIP6_Socket == INVALID_SOCKET) {
+        return;
+    }
+    /* leave a multicast address */
+    memcpy(
+        &broadcast_address.s6_addr[0], &BIP6_Broadcast_Addr.address[0],
+        IP6_ADDRESS_MAX);
+    memcpy(
+        &leave_request.ipv6mr_multiaddr, &broadcast_address,
+        sizeof(struct in6_addr));
+    status = setsockopt(
+        BIP6_Socket, IPPROTO_IPV6, IPV6_LEAVE_GROUP, (char *)&leave_request,
+        sizeof(leave_request));
+    if (status < 0) {
+        debug_fprintf(
+            stderr,
+            "BIP6: setsockopt(IPV6_LEAVE_GROUP) failed "
+            "with error %d: %s\n",
+            WSAGetLastError(), PrintError(WSAGetLastError()));
+    }
+}
+
 /** Initialize the BACnet/IP services at the given interface.
  * @ingroup DLBIP6
  * -# Gets the local IP address and local broadcast address from the system,
@@ -523,7 +568,7 @@ bool bip6_init(char *ifname)
 
     /* Ask for Winsock version 2.2. */
     if ((RetVal = WSAStartup(MAKEWORD(2, 2), &wd)) != 0) {
-        fprintf(
+        debug_fprintf(
             stderr, "BIP6: WSAStartup failed with error %d: %s\n", RetVal,
             PrintError(RetVal));
         WSACleanup();
@@ -543,7 +588,8 @@ bool bip6_init(char *ifname)
             BIP6_MULTICAST_GROUP_ID);
     }
     if (BIP6_Socket == INVALID_SOCKET) {
-        fprintf(stderr, "BIP6: Fatal error: unable to serve on any address.\n");
+        debug_fprintf(
+            stderr, "BIP6: Fatal error: unable to serve on any address.\n");
         WSACleanup();
         exit(1);
     }

src/bacnet/basic/bbmd6/h_bbmd6.c

@@ -1059,6 +1059,7 @@ int bvlc6_handler(
 int bvlc6_register_with_bbmd(
     const BACNET_IP6_ADDRESS *bbmd_addr, uint16_t ttl_seconds)
 {
+    int len;
     uint8_t mtu[BIP6_MPDU_MAX] = { 0 };
     uint16_t mtu_len = 0;
     uint32_t vmac_src = 0;
@@ -1070,8 +1071,12 @@ int bvlc6_register_with_bbmd(
     vmac_src = Device_Object_Instance_Number();
     mtu_len = bvlc6_encode_register_foreign_device(
         &mtu[0], sizeof(mtu), vmac_src, ttl_seconds);
+    len = bip6_send_mpdu(bbmd_addr, &mtu[0], mtu_len);
+    if (len > 0) {
+        bip6_leave_group();
+    }
 
-    return bip6_send_mpdu(bbmd_addr, &mtu[0], mtu_len);
+    return len;
 }
 
 /** Get the remote BBMD address that was used to Register as a foreign device

src/bacnet/datalink/bip6.h

@@ -39,6 +39,10 @@ bool bip6_init(char *ifname);
 BACNET_STACK_EXPORT
 void bip6_cleanup(void);
 BACNET_STACK_EXPORT
+void bip6_join_group(void);
+BACNET_STACK_EXPORT
+void bip6_leave_group(void);
+BACNET_STACK_EXPORT
 void bip6_get_broadcast_address(BACNET_ADDRESS *my_address);
 BACNET_STACK_EXPORT
 void bip6_get_my_address(BACNET_ADDRESS *my_address);

test/bacnet/basic/bbmd6/src/main.c

@@ -97,6 +97,14 @@ int bip6_send_mpdu(
     return 0;
 }
 
+/**
+ * @brief Leave a multicast group
+ */
+void bip6_leave_group(void)
+{
+    return;
+}
+
 /** Return the Object Instance number for our (single) Device Object.
  * This is a key function, widely invoked by the handler code, since
  * it provides "our" (ie, local) address.

test/bacnet/datalink/mock/src/bip6-mock.c

@@ -17,6 +17,14 @@ bool bip6_init(char *ifname)
     return ztest_get_return_value();
 }
 
+void bip6_join_group(void)
+{
+}
+
+void bip6_leave_group(void)
+{
+}
+
 void bip6_cleanup(void)
 {
 }