Skip to content

Commit

Permalink
AP_HAL: implement multicast for UDP sockets
Browse files Browse the repository at this point in the history
  • Loading branch information
@tridge @magicrub
tridge authored and magicrub committed Nov 25, 2023
1 parent 3f76a55 commit ee59247
Show file tree
Hide file tree
Showing 2 changed files with 133 additions and 57 deletions.
175 changes: 118 additions & 57 deletions libraries/AP_HAL/utility/Socket.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,37 +23,43 @@
#include "Socket.h"
#include <errno.h>

#if AP_NETWORKING_BACKEND_CHIBIOS
#define CALL_PREFIX(x) ::lwip_##x
#else
#define CALL_PREFIX(x) ::x
#endif

/*
constructor
*/
SocketAPM::SocketAPM(bool _datagram) :
SocketAPM(_datagram,
socket(AF_INET, _datagram?SOCK_DGRAM:SOCK_STREAM, 0))
CALL_PREFIX(socket)(AF_INET, _datagram?SOCK_DGRAM:SOCK_STREAM, 0))
{}

SocketAPM::SocketAPM(bool _datagram, int _fd) :
datagram(_datagram),
fd(_fd)
{
#ifdef FD_CLOEXEC
fcntl(fd, F_SETFD, FD_CLOEXEC);
CALL_PREFIX(fcntl)(fd, F_SETFD, FD_CLOEXEC);
#endif
if (!datagram) {
int one = 1;
setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
CALL_PREFIX(setsockopt)(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
}
}

SocketAPM::~SocketAPM()
{
if (fd != -1) {
#if AP_NETWORKING_BACKEND_CHIBIOS
::lwip_close(fd);
#else
::close(fd);
#endif
CALL_PREFIX(close)(fd);
fd = -1;
}
if (fd_in != -1) {
CALL_PREFIX(close)(fd_in);
fd_in = -1;
}
}

void SocketAPM::make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr)
Expand All @@ -74,16 +80,58 @@ void SocketAPM::make_sockaddr(const char *address, uint16_t port, struct sockadd
bool SocketAPM::connect(const char *address, uint16_t port)
{
struct sockaddr_in sockaddr;
int ret;
make_sockaddr(address, port, sockaddr);

#if AP_NETWORKING_BACKEND_CHIBIOS
if (::lwip_connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
#else
if (::connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
if (datagram && is_multicast_address(sockaddr)) {
/*
connect fd_in as a multicast UDP socket
*/
fd_in = CALL_PREFIX(socket)(AF_INET, SOCK_DGRAM, 0);
if (fd_in == -1) {
return false;
}
struct sockaddr_in sockaddr_mc = sockaddr;
int one = 1;
struct ip_mreq mreq {};
#ifdef FD_CLOEXEC
CALL_PREFIX(fcntl)(fd_in, F_SETFD, FD_CLOEXEC);
#endif
IGNORE_RETURN(CALL_PREFIX(setsockopt)(fd_in, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)));

#if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD)
/*
on cygwin you need to bind to INADDR_ANY then use the multicast
IP_ADD_MEMBERSHIP to get on the right address
*/
sockaddr_mc.sin_addr.s_addr = htonl(INADDR_ANY);
#endif

ret = CALL_PREFIX(bind)(fd_in, (struct sockaddr *)&sockaddr_mc, sizeof(sockaddr));
if (ret == -1) {
goto fail_mc;
}

mreq.imr_multiaddr.s_addr = sockaddr.sin_addr.s_addr;
mreq.imr_interface.s_addr = htonl(INADDR_ANY);

ret = CALL_PREFIX(setsockopt)(fd_in, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
if (ret == -1) {
goto fail_mc;
}
}

ret = CALL_PREFIX(connect)(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret != 0) {
return false;
}
connected = true;
return true;

fail_mc:
CALL_PREFIX(close)(fd_in);
fd_in = -1;
return false;
}

/*
Expand All @@ -96,11 +144,7 @@ bool SocketAPM::connect_timeout(const char *address, uint16_t port, uint32_t tim

set_blocking(false);

#if AP_NETWORKING_BACKEND_CHIBIOS
int ret = ::lwip_connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
#else
int ret = ::connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
#endif
int ret = CALL_PREFIX(connect)(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
if (ret == 0) {
// instant connect?
return true;
Expand All @@ -117,7 +161,8 @@ bool SocketAPM::connect_timeout(const char *address, uint16_t port, uint32_t tim
if (getsockopt(fd, SOL_SOCKET, SO_ERROR, (void*)&sock_error, &len) != 0) {
return false;
}
return sock_error == 0;
connected = sock_error == 0;
return connected;
}

/*
Expand All @@ -128,11 +173,7 @@ bool SocketAPM::bind(const char *address, uint16_t port)
struct sockaddr_in sockaddr;
make_sockaddr(address, port, sockaddr);

#if AP_NETWORKING_BACKEND_CHIBIOS
if (::lwip_bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
#else
if (::bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
#endif
if (CALL_PREFIX(bind)(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
return false;
}
return true;
Expand All @@ -145,7 +186,7 @@ bool SocketAPM::bind(const char *address, uint16_t port)
bool SocketAPM::reuseaddress(void) const
{
int one = 1;
return (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) != -1);
return (CALL_PREFIX(setsockopt)(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) != -1);
}

/*
Expand All @@ -155,9 +196,15 @@ bool SocketAPM::set_blocking(bool blocking) const
{
int fcntl_ret;
if (blocking) {
fcntl_ret = fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) & ~O_NONBLOCK);
fcntl_ret = CALL_PREFIX(fcntl)(fd, F_SETFL, CALL_PREFIX(fcntl)(fd, F_GETFL, 0) & ~O_NONBLOCK);
if (fd_in != -1) {
fcntl_ret |= CALL_PREFIX(fcntl)(fd_in, F_SETFL, CALL_PREFIX(fcntl)(fd_in, F_GETFL, 0) & ~O_NONBLOCK);
}
} else {
fcntl_ret = fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK);
fcntl_ret = CALL_PREFIX(fcntl)(fd, F_SETFL, CALL_PREFIX(fcntl)(fd, F_GETFL, 0) | O_NONBLOCK);
if (fd_in != -1) {
fcntl_ret |= CALL_PREFIX(fcntl)(fd_in, F_SETFL, CALL_PREFIX(fcntl)(fd_in, F_GETFL, 0) | O_NONBLOCK);
}
}
return fcntl_ret != -1;
}
Expand All @@ -168,7 +215,7 @@ bool SocketAPM::set_blocking(bool blocking) const
bool SocketAPM::set_cloexec() const
{
#ifdef FD_CLOEXEC
return (fcntl(fd, F_SETFD, FD_CLOEXEC) != -1);
return (CALL_PREFIX(fcntl)(fd, F_SETFD, FD_CLOEXEC) != -1);
#else
return false;
#endif
Expand All @@ -179,11 +226,7 @@ bool SocketAPM::set_cloexec() const
*/
ssize_t SocketAPM::send(const void *buf, size_t size) const
{
#if AP_NETWORKING_BACKEND_CHIBIOS
return ::lwip_send(fd, buf, size, 0);
#else
return ::send(fd, buf, size, 0);
#endif
return CALL_PREFIX(send)(fd, buf, size, 0);
}

/*
Expand All @@ -193,11 +236,7 @@ ssize_t SocketAPM::sendto(const void *buf, size_t size, const char *address, uin
{
struct sockaddr_in sockaddr;
make_sockaddr(address, port, sockaddr);
#if AP_NETWORKING_BACKEND_CHIBIOS
return ::lwip_sendto(fd, buf, size, 0, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
#else
return ::sendto(fd, buf, size, 0, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
#endif
return CALL_PREFIX(sendto)(fd, buf, size, 0, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
}

/*
Expand All @@ -209,11 +248,29 @@ ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms)
return -1;
}
socklen_t len = sizeof(in_addr);
#if AP_NETWORKING_BACKEND_CHIBIOS
return ::lwip_recvfrom(fd, buf, size, MSG_DONTWAIT, (sockaddr *)&in_addr, &len);
#else
return ::recvfrom(fd, buf, size, MSG_DONTWAIT, (sockaddr *)&in_addr, &len);
#endif
int fin = get_read_fd();
ssize_t ret;
ret = CALL_PREFIX(recvfrom)(fin, buf, size, MSG_DONTWAIT, (sockaddr *)&in_addr, &len);
if (ret <= 0) {
return ret;
}
if (fd_in != -1) {
/*
for multicast check we are not receiving from ourselves
*/
struct sockaddr_in send_addr;
socklen_t send_len = sizeof(send_addr);
if (CALL_PREFIX(getsockname)(fd, (struct sockaddr *)&send_addr, &send_len) != 0) {
return -1;
}
if (in_addr.sin_port == send_addr.sin_port &&
in_addr.sin_family == send_addr.sin_family &&
in_addr.sin_addr.s_addr == send_addr.sin_addr.s_addr) {
// discard packets from ourselves
return -1;
}
}
return ret;
}

/*
Expand All @@ -228,7 +285,7 @@ void SocketAPM::last_recv_address(const char *&ip_addr, uint16_t &port) const
void SocketAPM::set_broadcast(void) const
{
int one = 1;
setsockopt(fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one));
CALL_PREFIX(setsockopt)(fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one));
}

/*
Expand All @@ -240,12 +297,13 @@ bool SocketAPM::pollin(uint32_t timeout_ms)
struct timeval tv;

FD_ZERO(&fds);
FD_SET(fd, &fds);
int fin = get_read_fd();
FD_SET(fin, &fds);

tv.tv_sec = timeout_ms / 1000;
tv.tv_usec = (timeout_ms % 1000) * 1000UL;

if (select(fd+1, &fds, nullptr, nullptr, &tv) != 1) {
if (CALL_PREFIX(select)(fin+1, &fds, nullptr, nullptr, &tv) != 1) {
return false;
}
return true;
Expand All @@ -266,7 +324,7 @@ bool SocketAPM::pollout(uint32_t timeout_ms)
tv.tv_sec = timeout_ms / 1000;
tv.tv_usec = (timeout_ms % 1000) * 1000UL;

if (select(fd+1, nullptr, &fds, nullptr, &tv) != 1) {
if (CALL_PREFIX(select)(fd+1, nullptr, &fds, nullptr, &tv) != 1) {
return false;
}
return true;
Expand All @@ -277,11 +335,7 @@ bool SocketAPM::pollout(uint32_t timeout_ms)
*/
bool SocketAPM::listen(uint16_t backlog) const
{
#if AP_NETWORKING_BACKEND_CHIBIOS
return ::lwip_listen(fd, (int)backlog) == 0;
#else
return ::listen(fd, (int)backlog) == 0;
#endif
return CALL_PREFIX(listen)(fd, (int)backlog) == 0;
}

/*
Expand All @@ -294,18 +348,25 @@ SocketAPM *SocketAPM::accept(uint32_t timeout_ms)
return nullptr;
}

#if AP_NETWORKING_BACKEND_CHIBIOS
int newfd = ::lwip_accept(fd, nullptr, nullptr);
#else
int newfd = ::accept(fd, nullptr, nullptr);
#endif
int newfd = CALL_PREFIX(accept)(fd, nullptr, nullptr);
if (newfd == -1) {
return nullptr;
}
// turn off nagle for lower latency
int one = 1;
setsockopt(newfd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
CALL_PREFIX(setsockopt)(newfd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
return new SocketAPM(false, newfd);
}

/*
return true if an address is in the multicast range
*/
bool SocketAPM::is_multicast_address(struct sockaddr_in &addr) const
{
const uint32_t mc_lower = 0xE0000000; // 224.0.0.0
const uint32_t mc_upper = 0xEFFFFFFF; // 239.255.255.255
const uint32_t haddr = ntohl(addr.sin_addr.s_addr);
return haddr >= mc_lower && haddr <= mc_upper;
}

#endif // AP_NETWORKING_BACKEND_ANY
15 changes: 15 additions & 0 deletions libraries/AP_HAL/utility/Socket.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,12 +70,27 @@ class SocketAPM {
// listen has been used. A new socket is returned
SocketAPM *accept(uint32_t timeout_ms);

// get a FD suitable for read selection
int get_read_fd(void) const {
return fd_in != -1? fd_in : fd;
}

bool is_connected(void) const {
return connected;
}

private:
bool datagram;
struct sockaddr_in in_addr {};
bool is_multicast_address(struct sockaddr_in &addr) const;

int fd = -1;

// fd_in is used for multicast UDP
int fd_in = -1;

bool connected;

void make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr);
};

Expand Down

0 comments on commit ee59247

Please sign in to comment.