diff --git a/include/zenoh-pico/link/config/udp.h b/include/zenoh-pico/link/config/udp.h index 36e5b08c9..877896493 100644 --- a/include/zenoh-pico/link/config/udp.h +++ b/include/zenoh-pico/link/config/udp.h @@ -20,7 +20,7 @@ #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" @@ -28,12 +28,17 @@ #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 index 33a466180..a154c20f0 100644 --- 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 index 17761dda8..3629f25be 100644 --- 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 index bc07494e8..981b1a37f 100644 --- 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 index 80462b794..c85ea7d07 100644 --- 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 index ed3042985..8dfecbcec 100644 --- 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 index f6a24bd0b..55b3e7036 100644 --- 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 index fa189f4b8..6653fceea 100644 --- 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 index cc00f6d3e..917e9a4be 100644 --- 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 index 3268a0890..6ce0eb5ab 100644 --- 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 index f0842dc08..49978b49c 100644 --- 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 index 3254c5f2a..6325d30fe 100644 --- 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