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/unix/network.c b/src/system/unix/network.c index cc00f6d3e..b63606947 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; @@ -443,6 +443,42 @@ int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpo } else { ret = _Z_ERR_GENERIC; } + if (join != NULL) { + char *joins = _z_str_clone(join); + struct addrinfo hints = {0}; + hints.ai_family = PF_UNSPEC; // Allow IPv4 or IPv6 + hints.ai_socktype = SOCK_DGRAM; + hints.ai_protocol = IPPROTO_UDP; + for (char *j = strsep(&joins, "|"); j != NULL; j = strsep(&joins, ";")) { + struct addrinfo *addr = NULL; + char *ip = strsep(&j, ":"); + if (getaddrinfo(ip, j, &hints, &addr) < 0) { + ret = _Z_ERR_GENERIC; + } + if (addr->ai_family == AF_INET) { + struct ip_mreq mreq; + (void)memset(&mreq, 0, sizeof(mreq)); + mreq.imr_multiaddr.s_addr = ((struct sockaddr_in *)addr->ai_addr)->sin_addr.s_addr; + 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 (addr->ai_family == AF_INET6) { + struct ipv6_mreq mreq; + (void)memset(&mreq, 0, sizeof(mreq)); + (void)memcpy(&mreq.ipv6mr_multiaddr, &((struct sockaddr_in6 *)addr->ai_addr)->sin6_addr, + sizeof(struct in6_addr)); + 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; + } + } + freeaddrinfo(addr); + } + z_free(joins); + } if (ret != _Z_RES_OK) { close(sock->_fd); 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