Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL: support bi-directional UDP broadcast sockets

Browse Source
This commit is contained in:
Andrew Tridgell 2023-11-28 09:22:54 +11:00
parent 7d1f048ca7
commit 2ee48dea29
1 changed files with 27 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_HAL/utility/Socket.cpp
View File

@ -81,6 +81,7 @@ bool SocketAPM::connect(const char *address, uint16_t port)
{ {
struct sockaddr_in sockaddr; struct sockaddr_in sockaddr;
int ret; int ret;
int one = 1;
make_sockaddr(address, port, sockaddr); make_sockaddr(address, port, sockaddr);
if (datagram && is_multicast_address(sockaddr)) { if (datagram && is_multicast_address(sockaddr)) {
@ -92,7 +93,6 @@ bool SocketAPM::connect(const char *address, uint16_t port)
return false; return false;
} }
struct sockaddr_in sockaddr_mc = sockaddr; struct sockaddr_in sockaddr_mc = sockaddr;
int one = 1;
struct ip_mreq mreq {}; struct ip_mreq mreq {};
#ifdef FD_CLOEXEC #ifdef FD_CLOEXEC
CALL_PREFIX(fcntl)(fd_in, F_SETFD, FD_CLOEXEC); CALL_PREFIX(fcntl)(fd_in, F_SETFD, FD_CLOEXEC);
@ -109,7 +109,7 @@ bool SocketAPM::connect(const char *address, uint16_t port)
ret = CALL_PREFIX(bind)(fd_in, (struct sockaddr *)&sockaddr_mc, sizeof(sockaddr)); ret = CALL_PREFIX(bind)(fd_in, (struct sockaddr *)&sockaddr_mc, sizeof(sockaddr));
if (ret == -1) { if (ret == -1) {
goto fail_mc; goto fail_multi;
} }
mreq.imr_multiaddr.s_addr = sockaddr.sin_addr.s_addr; mreq.imr_multiaddr.s_addr = sockaddr.sin_addr.s_addr;
@ -117,10 +117,14 @@ bool SocketAPM::connect(const char *address, uint16_t port)
ret = CALL_PREFIX(setsockopt)(fd_in, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); ret = CALL_PREFIX(setsockopt)(fd_in, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
if (ret == -1) { if (ret == -1) {
goto fail_mc; goto fail_multi;
} }
} else if (datagram && sockaddr.sin_addr.s_addr == INADDR_BROADCAST) { }
if (datagram && sockaddr.sin_addr.s_addr == INADDR_BROADCAST) {
// setup for bi-directional UDP broadcast
set_broadcast(); set_broadcast();
reuseaddress();
} }
ret = CALL_PREFIX(connect)(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); ret = CALL_PREFIX(connect)(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
@ -128,9 +132,27 @@ bool SocketAPM::connect(const char *address, uint16_t port)
return false; return false;
} }
connected = true; connected = true;
if (datagram && sockaddr.sin_addr.s_addr == INADDR_BROADCAST) {
// for bi-directional UDP broadcast we need 2 sockets
struct sockaddr_in send_addr;
socklen_t send_len = sizeof(send_addr);
ret = CALL_PREFIX(getsockname)(fd, (struct sockaddr *)&send_addr, &send_len);
fd_in = CALL_PREFIX(socket)(AF_INET, SOCK_DGRAM, 0);
if (fd_in == -1) {
goto fail_multi;
}
CALL_PREFIX(setsockopt)(fd_in, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
// 2nd socket needs to be bound to wildcard
send_addr.sin_addr.s_addr = INADDR_ANY;
ret = CALL_PREFIX(bind)(fd_in, (struct sockaddr *)&send_addr, sizeof(send_addr));
if (ret == -1) {
goto fail_multi;
}
}
return true; return true;
fail_mc: fail_multi:
CALL_PREFIX(close)(fd_in); CALL_PREFIX(close)(fd_in);
fd_in = -1; fd_in = -1;
return false; return false;