Skip to content

Commit a40ec89

Browse files
p-avitalp-avital
authored
Merge pull request #252 from eclipse-zenoh/udp-multijoin
udp multicast can now join multiple groups
2 parents ab442b9 + 7669c2f commit a40ec89

File tree

12 files changed

+67
-26
lines changed

12 files changed

+67
-26
lines changed

‎include/zenoh-pico/link/config/udp.h‎

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -20,20 +20,25 @@
2020

2121
#if Z_LINK_UDP_UNICAST == 1 || Z_LINK_UDP_MULTICAST == 1
2222

23-
#define UDP_CONFIG_ARGC 2
23+
#define UDP_CONFIG_ARGC 3
2424

2525
#define UDP_CONFIG_IFACE_KEY 0x01
2626
#define UDP_CONFIG_IFACE_STR "iface"
2727

2828
#define UDP_CONFIG_TOUT_KEY 0x02
2929
#define UDP_CONFIG_TOUT_STR "tout"
3030

31+
#define UDP_CONFIG_JOIN_KEY 0x03
32+
#define UDP_CONFIG_JOIN_STR "join"
33+
3134
#define UDP_CONFIG_MAPPING_BUILD \
3235
_z_str_intmapping_t args[UDP_CONFIG_ARGC]; \
3336
args[0]._key = UDP_CONFIG_IFACE_KEY; \
3437
args[0]._str = UDP_CONFIG_IFACE_STR; \
3538
args[1]._key = UDP_CONFIG_TOUT_KEY; \
36-
args[1]._str = UDP_CONFIG_TOUT_STR;
39+
args[1]._str = UDP_CONFIG_TOUT_STR; \
40+
args[2]._key = UDP_CONFIG_JOIN_KEY; \
41+
args[2]._str = UDP_CONFIG_JOIN_STR;
3742

3843
size_t _z_udp_config_strlen(const _z_str_intmap_t *s);
3944

‎include/zenoh-pico/system/link/udp.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ size_t _z_send_udp_unicast(const _z_sys_net_socket_t sock, const uint8_t *ptr, s
4545
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,
4646
uint32_t tout, const char *iface);
4747
int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
48-
const char *iface);
48+
const char *iface, const char *join);
4949
void _z_close_udp_multicast(_z_sys_net_socket_t *sockrecv, _z_sys_net_socket_t *socksend,
5050
const _z_sys_net_endpoint_t rep);
5151
size_t _z_read_exact_udp_multicast(const _z_sys_net_socket_t sock, uint8_t *ptr, size_t len,

‎src/link/config/udp.c‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,6 @@ char *_z_udp_config_to_str(const _z_str_intmap_t *s) {
4040

4141
int8_t _z_udp_config_from_strn(_z_str_intmap_t *strint, const char *s, size_t n) {
4242
UDP_CONFIG_MAPPING_BUILD
43-
4443
return _z_str_intmap_from_strn(strint, s, UDP_CONFIG_ARGC, args, n);
4544
}
4645

‎src/link/multicast/udp.c‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -129,7 +129,9 @@ int8_t _z_f_link_listen_udp_multicast(_z_link_t *self) {
129129
int8_t ret = _Z_RES_OK;
130130

131131
const char *iface = _z_str_intmap_get(&self->_endpoint._config, UDP_CONFIG_IFACE_KEY);
132-
ret = _z_listen_udp_multicast(&self->_socket._udp._sock, self->_socket._udp._rep, Z_CONFIG_SOCKET_TIMEOUT, iface);
132+
const char *join = _z_str_intmap_get(&self->_endpoint._config, UDP_CONFIG_JOIN_KEY);
133+
ret = _z_listen_udp_multicast(&self->_socket._udp._sock, self->_socket._udp._rep, Z_CONFIG_SOCKET_TIMEOUT, iface,
134+
join);
133135
ret |= _z_open_udp_multicast(&self->_socket._udp._msock, self->_socket._udp._rep, &self->_socket._udp._lep,
134136
Z_CONFIG_SOCKET_TIMEOUT, iface);
135137

‎src/system/arduino/esp32/network.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -346,7 +346,8 @@ int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoin
346346
}
347347

348348
int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
349-
const char *iface) {
349+
const char *iface, const char *join) {
350+
(void)join;
350351
int8_t ret = _Z_RES_OK;
351352

352353
struct sockaddr *lsockaddr = NULL;

‎src/system/arduino/opencr/network.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -252,7 +252,8 @@ int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoin
252252
}
253253

254254
int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
255-
const char *iface) {
255+
const char *iface, const char *join) {
256+
(void)join;
256257
int8_t ret = _Z_RES_OK;
257258

258259
sock->_udp = new WiFiUDP();

‎src/system/espidf/network.c‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -342,7 +342,8 @@ int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoin
342342
}
343343

344344
int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
345-
const char *iface) {
345+
const char *iface, const char *join) {
346+
(void)join;
346347
int8_t ret = _Z_RES_OK;
347348

348349
struct sockaddr *lsockaddr = NULL;

‎src/system/mbed/network.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -225,7 +225,8 @@ int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoin
225225
}
226226

227227
int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
228-
const char *iface) {
228+
const char *iface, const char *join) {
229+
(void)join;
229230
int8_t ret = _Z_RES_OK;
230231

231232
sock->_udp = new UDPSocket();

‎src/system/unix/network.c‎

Lines changed: 43 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -378,7 +378,7 @@ int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoin
378378
}
379379

380380
int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
381-
const char *iface) {
381+
const char *iface, const char *join) {
382382
int8_t ret = _Z_RES_OK;
383383

384384
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
393393
ret = _Z_ERR_GENERIC;
394394
}
395395

396-
int optflag = 1;
397-
if ((ret == _Z_RES_OK) &&
398-
(setsockopt(sock->_fd, SOL_SOCKET, SO_REUSEADDR, (char *)&optflag, sizeof(optflag)) < 0)) {
396+
int value = true;
397+
if ((ret == _Z_RES_OK) && (setsockopt(sock->_fd, SOL_SOCKET, SO_REUSEADDR, &value, sizeof(value)) < 0)) {
399398
ret = _Z_ERR_GENERIC;
400399
}
401-
402-
#if defined(ZENOH_MACOS) || defined(ZENOH_BSD)
403-
if ((ret == _Z_RES_OK) && (bind(sock->_fd, rep._iptcp->ai_addr, rep._iptcp->ai_addrlen) < 0)) {
400+
if ((ret == _Z_RES_OK) && (setsockopt(sock->_fd, SOL_SOCKET, SO_REUSEPORT, &value, sizeof(value)) < 0)) {
404401
ret = _Z_ERR_GENERIC;
405402
}
406-
#elif defined(ZENOH_LINUX)
403+
407404
if (rep._iptcp->ai_family == AF_INET) {
408-
struct sockaddr_in address = {AF_INET, ((struct sockaddr_in *)rep._iptcp->ai_addr)->sin_port, {0}, {0}};
409-
if ((ret == _Z_RES_OK) && (bind(sock->_fd, (struct sockaddr *)&address, sizeof address) < 0)) {
405+
struct sockaddr_in address;
406+
(void)memset(&address, 0, sizeof(address));
407+
address.sin_family = rep._iptcp->ai_family;
408+
address.sin_port = ((struct sockaddr_in *)rep._iptcp->ai_addr)->sin_port;
409+
inet_pton(address.sin_family, "0.0.0.0", &address.sin_addr);
410+
if ((ret == _Z_RES_OK) && (bind(sock->_fd, (struct sockaddr *)&address, sizeof(address)) < 0)) {
410411
ret = _Z_ERR_GENERIC;
411412
}
412413
} else if (rep._iptcp->ai_family == AF_INET6) {
413-
struct sockaddr_in6 address = {
414-
AF_INET6, ((struct sockaddr_in6 *)rep._iptcp->ai_addr)->sin6_port, 0, {{{0}}}, 0};
415-
if ((ret == _Z_RES_OK) && (bind(sock->_fd, (struct sockaddr *)&address, sizeof address) < 0)) {
414+
struct sockaddr_in6 address;
415+
(void)memset(&address, 0, sizeof(address));
416+
address.sin6_family = rep._iptcp->ai_family;
417+
address.sin6_port = ((struct sockaddr_in6 *)rep._iptcp->ai_addr)->sin6_port;
418+
inet_pton(address.sin6_family, "::", &address.sin6_addr);
419+
if ((ret == _Z_RES_OK) && (bind(sock->_fd, (struct sockaddr *)&address, sizeof(address)) < 0)) {
416420
ret = _Z_ERR_GENERIC;
417421
}
418422
} else {
419423
ret = _Z_ERR_GENERIC;
420424
}
421-
#endif
422425

423426
// Join the multicast group
424427
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
443446
} else {
444447
ret = _Z_ERR_GENERIC;
445448
}
449+
// Join any additional multicast group
450+
if (join != NULL) {
451+
char *joins = _z_str_clone(join);
452+
for (char *ip = strsep(&joins, "|"); ip != NULL; ip = strsep(&joins, "|")) {
453+
if (rep._iptcp->ai_family == AF_INET) {
454+
struct ip_mreq mreq;
455+
(void)memset(&mreq, 0, sizeof(mreq));
456+
inet_pton(rep._iptcp->ai_family, ip, &mreq.imr_multiaddr);
457+
mreq.imr_interface.s_addr = ((struct sockaddr_in *)lsockaddr)->sin_addr.s_addr;
458+
if ((ret == _Z_RES_OK) &&
459+
(setsockopt(sock->_fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0)) {
460+
ret = _Z_ERR_GENERIC;
461+
}
462+
} else if (rep._iptcp->ai_family == AF_INET6) {
463+
struct ipv6_mreq mreq;
464+
(void)memset(&mreq, 0, sizeof(mreq));
465+
inet_pton(rep._iptcp->ai_family, ip, &mreq.ipv6mr_multiaddr);
466+
mreq.ipv6mr_interface = if_nametoindex(iface);
467+
if ((ret == _Z_RES_OK) &&
468+
(setsockopt(sock->_fd, IPPROTO_IPV6, IPV6_JOIN_GROUP, &mreq, sizeof(mreq)) < 0)) {
469+
ret = _Z_ERR_GENERIC;
470+
}
471+
}
472+
}
473+
z_free(joins);
474+
}
446475

447476
if (ret != _Z_RES_OK) {
448477
close(sock->_fd);

‎src/system/windows/network.c‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -417,7 +417,8 @@ int8_t _z_open_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoin
417417
}
418418

419419
int8_t _z_listen_udp_multicast(_z_sys_net_socket_t *sock, const _z_sys_net_endpoint_t rep, uint32_t tout,
420-
const char *iface) {
420+
const char *iface, const char *join) {
421+
(void)join;
421422
int8_t ret = _Z_RES_OK;
422423

423424
if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {

0 commit comments

Comments
 (0)