Merge pull request #252 from eclipse-zenoh/udp-multijoin

udp multicast can now join multiple groups
eclipse-zenoh · Sep 20, 2023 · a40ec89 · a40ec89
2 parents ab442b9 + 7669c2f
commit a40ec89
Show file tree

Hide file tree

Showing 12 changed files with 67 additions and 26 deletions.
diff --git a/include/zenoh-pico/link/config/udp.h b/include/zenoh-pico/link/config/udp.h
@@ -20,20 +20,25 @@
 
 #if Z_LINK_UDP_UNICAST == 1 || Z_LINK_UDP_MULTICAST == 1
 
-#define UDP_CONFIG_ARGC 2
+#define UDP_CONFIG_ARGC 3
 
 #define UDP_CONFIG_IFACE_KEY 0x01
 #define UDP_CONFIG_IFACE_STR "iface"
 
 #define UDP_CONFIG_TOUT_KEY 0x02
 #define UDP_CONFIG_TOUT_STR "tout"
 
+#define UDP_CONFIG_JOIN_KEY 0x03
+#define UDP_CONFIG_JOIN_STR "join"
+
 #define UDP_CONFIG_MAPPING_BUILD               \
     _z_str_intmapping_t args[UDP_CONFIG_ARGC]; \
     args[0]._key = UDP_CONFIG_IFACE_KEY;       \
     args[0]._str = UDP_CONFIG_IFACE_STR;       \
     args[1]._key = UDP_CONFIG_TOUT_KEY;        \
-    args[1]._str = UDP_CONFIG_TOUT_STR;
+    args[1]._str = UDP_CONFIG_TOUT_STR;        \
+    args[2]._key = UDP_CONFIG_JOIN_KEY;        \
+    args[2]._str = UDP_CONFIG_JOIN_STR;
 
 size_t _z_udp_config_strlen(const _z_str_intmap_t *s);
 

diff --git a/include/zenoh-pico/system/link/udp.h b/include/zenoh-pico/system/link/udp.h
@@ -45,7 +45,7 @@ size_t _z_send_udp_unicast(const _z_sys_net_socket_t sock, const uint8_t *ptr, s
 int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, _z_sys_net_endpoint_t *lep,
                              uint32_t tout, const char *iface);
 int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
-                               const char *iface);
+                               const char *iface, const char *join);
 void _z_close_udp_multicast(_z_sys_net_socket_t *sockrecv, _z_sys_net_socket_t *socksend,
                             const _z_sys_net_endpoint_t rep);
 size_t _z_read_exact_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len,

diff --git a/src/link/config/udp.c b/src/link/config/udp.c
@@ -40,7 +40,6 @@ char *_z_udp_config_to_str(const _z_str_intmap_t *s) {
 
 int8_t _z_udp_config_from_strn(_z_str_intmap_t *strint, const char *s, size_t n) {
     UDP_CONFIG_MAPPING_BUILD
-
     return _z_str_intmap_from_strn(strint, s, UDP_CONFIG_ARGC, args, n);
 }
 

diff --git a/src/link/multicast/udp.c b/src/link/multicast/udp.c
@@ -129,7 +129,9 @@ int8_t _z_f_link_listen_udp_multicast(_z_link_t *self) {
     int8_t ret = _Z_RES_OK;
 
     const char *iface = _z_str_intmap_get(&self->_endpoint._config, UDP_CONFIG_IFACE_KEY);
-    ret = _z_listen_udp_multicast(&self->_socket._udp._sock, self->_socket._udp._rep, Z_CONFIG_SOCKET_TIMEOUT, iface);
+    const char *join = _z_str_intmap_get(&self->_endpoint._config, UDP_CONFIG_JOIN_KEY);
+    ret = _z_listen_udp_multicast(&self->_socket._udp._sock, self->_socket._udp._rep, Z_CONFIG_SOCKET_TIMEOUT, iface,
+                                  join);
     ret |= _z_open_udp_multicast(&self->_socket._udp._msock, self->_socket._udp._rep, &self->_socket._udp._lep,
                                  Z_CONFIG_SOCKET_TIMEOUT, iface);
 

diff --git a/src/system/arduino/esp32/network.cpp b/src/system/arduino/esp32/network.cpp
@@ -346,7 +346,8 @@ int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoin
 }
 
 int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
-                               const char *iface) {
+                               const char *iface, const char *join) {
+    (void)join;
     int8_t ret = _Z_RES_OK;
 
     struct sockaddr *lsockaddr = NULL;

diff --git a/src/system/arduino/opencr/network.cpp b/src/system/arduino/opencr/network.cpp
@@ -252,7 +252,8 @@ int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoin
 }
 
 int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
-                               const char *iface) {
+                               const char *iface, const char *join) {
+    (void)join;
     int8_t ret = _Z_RES_OK;
 
     sock->_udp = new WiFiUDP();

diff --git a/src/system/espidf/network.c b/src/system/espidf/network.c
@@ -342,7 +342,8 @@ int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoin
 }
 
 int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
-                               const char *iface) {
+                               const char *iface, const char *join) {
+    (void)join;
     int8_t ret = _Z_RES_OK;
 
     struct sockaddr *lsockaddr = NULL;

diff --git a/src/system/mbed/network.cpp b/src/system/mbed/network.cpp
@@ -225,7 +225,8 @@ int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoin
 }
 
 int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
-                               const char *iface) {
+                               const char *iface, const char *join) {
+    (void)join;
     int8_t ret = _Z_RES_OK;
 
     sock->_udp = new UDPSocket();

diff --git a/src/system/unix/network.c b/src/system/unix/network.c
@@ -378,7 +378,7 @@ int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoin
 }
 
 int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
-                               const char *iface) {
+                               const char *iface, const char *join) {
     int8_t ret = _Z_RES_OK;
 
     struct sockaddr *lsockaddr = NULL;
@@ -393,32 +393,35 @@ int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpo
                 ret = _Z_ERR_GENERIC;
             }
 
-            int optflag = 1;
-            if ((ret == _Z_RES_OK) &&
-                (setsockopt(sock->_fd, SOL_SOCKET, SO_REUSEADDR, (char *)&optflag, sizeof(optflag)) < 0)) {
+            int value = true;
+            if ((ret == _Z_RES_OK) && (setsockopt(sock->_fd, SOL_SOCKET, SO_REUSEADDR, &value, sizeof(value)) < 0)) {
                 ret = _Z_ERR_GENERIC;
             }
-
-#if defined(ZENOH_MACOS) || defined(ZENOH_BSD)
-            if ((ret == _Z_RES_OK) && (bind(sock->_fd, rep._iptcp->ai_addr, rep._iptcp->ai_addrlen) < 0)) {
+            if ((ret == _Z_RES_OK) && (setsockopt(sock->_fd, SOL_SOCKET, SO_REUSEPORT, &value, sizeof(value)) < 0)) {
                 ret = _Z_ERR_GENERIC;
             }
-#elif defined(ZENOH_LINUX)
+
             if (rep._iptcp->ai_family == AF_INET) {
-                struct sockaddr_in address = {AF_INET, ((struct sockaddr_in *)rep._iptcp->ai_addr)->sin_port, {0}, {0}};
-                if ((ret == _Z_RES_OK) && (bind(sock->_fd, (struct sockaddr *)&address, sizeof address) < 0)) {
+                struct sockaddr_in address;
+                (void)memset(&address, 0, sizeof(address));
+                address.sin_family = rep._iptcp->ai_family;
+                address.sin_port = ((struct sockaddr_in *)rep._iptcp->ai_addr)->sin_port;
+                inet_pton(address.sin_family, "0.0.0.0", &address.sin_addr);
+                if ((ret == _Z_RES_OK) && (bind(sock->_fd, (struct sockaddr *)&address, sizeof(address)) < 0)) {
                     ret = _Z_ERR_GENERIC;
                 }
             } else if (rep._iptcp->ai_family == AF_INET6) {
-                struct sockaddr_in6 address = {
-                    AF_INET6, ((struct sockaddr_in6 *)rep._iptcp->ai_addr)->sin6_port, 0, {{{0}}}, 0};
-                if ((ret == _Z_RES_OK) && (bind(sock->_fd, (struct sockaddr *)&address, sizeof address) < 0)) {
+                struct sockaddr_in6 address;
+                (void)memset(&address, 0, sizeof(address));
+                address.sin6_family = rep._iptcp->ai_family;
+                address.sin6_port = ((struct sockaddr_in6 *)rep._iptcp->ai_addr)->sin6_port;
+                inet_pton(address.sin6_family, "::", &address.sin6_addr);
+                if ((ret == _Z_RES_OK) && (bind(sock->_fd, (struct sockaddr *)&address, sizeof(address)) < 0)) {
                     ret = _Z_ERR_GENERIC;
                 }
             } else {
                 ret = _Z_ERR_GENERIC;
             }
-#endif
 
             // Join the multicast group
             if (rep._iptcp->ai_family == AF_INET) {
@@ -443,6 +446,32 @@ int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpo
             } else {
                 ret = _Z_ERR_GENERIC;
             }
+            // Join any additional multicast group
+            if (join != NULL) {
+                char *joins = _z_str_clone(join);
+                for (char *ip = strsep(&joins, "|"); ip != NULL; ip = strsep(&joins, "|")) {
+                    if (rep._iptcp->ai_family == AF_INET) {
+                        struct ip_mreq mreq;
+                        (void)memset(&mreq, 0, sizeof(mreq));
+                        inet_pton(rep._iptcp->ai_family, ip, &mreq.imr_multiaddr);
+                        mreq.imr_interface.s_addr = ((struct sockaddr_in *)lsockaddr)->sin_addr.s_addr;
+                        if ((ret == _Z_RES_OK) &&
+                            (setsockopt(sock->_fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0)) {
+                            ret = _Z_ERR_GENERIC;
+                        }
+                    } else if (rep._iptcp->ai_family == AF_INET6) {
+                        struct ipv6_mreq mreq;
+                        (void)memset(&mreq, 0, sizeof(mreq));
+                        inet_pton(rep._iptcp->ai_family, ip, &mreq.ipv6mr_multiaddr);
+                        mreq.ipv6mr_interface = if_nametoindex(iface);
+                        if ((ret == _Z_RES_OK) &&
+                            (setsockopt(sock->_fd, IPPROTO_IPV6, IPV6_JOIN_GROUP, &mreq, sizeof(mreq)) < 0)) {
+                            ret = _Z_ERR_GENERIC;
+                        }
+                    }
+                }
+                z_free(joins);
+            }
 
             if (ret != _Z_RES_OK) {
                 close(sock->_fd);

diff --git a/src/system/windows/network.c b/src/system/windows/network.c
@@ -417,7 +417,8 @@ int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoin
 }
 
 int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
-                               const char *iface) {
+                               const char *iface, const char *join) {
+    (void)join;
     int8_t ret = _Z_RES_OK;
 
     if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {

diff --git a/src/system/zephyr/network.c b/src/system/zephyr/network.c
@@ -337,7 +337,8 @@ int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoin
 }
 
 int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
-                               const char *iface) {
+                               const char *iface, const char *join) {
+    (void)join;
     int8_t ret = _Z_RES_OK;
 
     struct sockaddr *lsockaddr = NULL;

diff --git a/zenohpico.pc b/zenohpico.pc
@@ -3,6 +3,6 @@ prefix=/usr/local
 Name: zenohpico
 Description: 
 URL: 
-Version: 0.10.20230915dev
+Version: 0.10.20230920dev
 Cflags: -I${prefix}/
 Libs: -L${prefix}/ -lzenohpico