diff --git a/libraries/AP_HAL/utility/Socket.cpp b/libraries/AP_HAL/utility/Socket.cpp index 363b144d67..0bad1ba6a9 100644 --- a/libraries/AP_HAL/utility/Socket.cpp +++ b/libraries/AP_HAL/utility/Socket.cpp @@ -21,9 +21,23 @@ #if AP_NETWORKING_SOCKETS_ENABLED #include "Socket.h" +#if AP_NETWORKING_BACKEND_CHIBIOS || AP_NETWORKING_BACKEND_PPP +#include +#else +// SITL or Linux +#include +#include +#include +#include +#include +#include +#include +#include +#endif + #include -#if AP_NETWORKING_BACKEND_CHIBIOS +#if AP_NETWORKING_BACKEND_CHIBIOS || AP_NETWORKING_BACKEND_PPP #define CALL_PREFIX(x) ::lwip_##x #else #define CALL_PREFIX(x) ::x @@ -33,6 +47,8 @@ #define MSG_NOSIGNAL 0 #endif +static_assert(sizeof(last_in_addr) == sizeof(struct sockaddr_in), "last_in_addr must match sockaddr_in size"); + /* constructor */ @@ -185,7 +201,7 @@ bool SocketAPM::connect_timeout(const char *address, uint16_t port, uint32_t tim } int sock_error = 0; socklen_t len = sizeof(sock_error); - if (getsockopt(fd, SOL_SOCKET, SO_ERROR, (void*)&sock_error, &len) != 0) { + if (CALL_PREFIX(getsockopt)(fd, SOL_SOCKET, SO_ERROR, (void*)&sock_error, &len) != 0) { return false; } connected = sock_error == 0; @@ -294,10 +310,10 @@ ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms) errno = EWOULDBLOCK; return -1; } - socklen_t len = sizeof(in_addr); + socklen_t len = sizeof(last_in_addr); int fin = get_read_fd(); ssize_t ret; - ret = CALL_PREFIX(recvfrom)(fin, buf, size, MSG_DONTWAIT, (sockaddr *)&in_addr, &len); + ret = CALL_PREFIX(recvfrom)(fin, buf, size, MSG_DONTWAIT, (sockaddr *)&last_in_addr, &len); if (ret <= 0) { if (!datagram && connected && ret == 0) { // remote host has closed connection @@ -314,9 +330,9 @@ ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms) 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) { + if (last_in_addr.sin_port == send_addr.sin_port && + last_in_addr.sin_family == send_addr.sin_family && + last_in_addr.sin_addr.s_addr == send_addr.sin_addr.s_addr) { // discard packets from ourselves return -1; } @@ -329,8 +345,9 @@ ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms) */ void SocketAPM::last_recv_address(const char *&ip_addr, uint16_t &port) const { - ip_addr = inet_ntoa(in_addr.sin_addr); - port = ntohs(in_addr.sin_port); + static char buf[16]; + auto *str = last_recv_address(buf, sizeof(buf), port); + ip_addr = str; } /* @@ -338,11 +355,11 @@ void SocketAPM::last_recv_address(const char *&ip_addr, uint16_t &port) const */ const char *SocketAPM::last_recv_address(char *ip_addr_buf, uint8_t buflen, uint16_t &port) const { - const char *ret = inet_ntop(AF_INET, (void*)&in_addr.sin_addr, ip_addr_buf, buflen); + const char *ret = CALL_PREFIX(inet_ntop)(AF_INET, (void*)&last_in_addr.sin_addr, ip_addr_buf, buflen); if (ret == nullptr) { return nullptr; } - port = ntohs(in_addr.sin_port); + port = ntohs(last_in_addr.sin_port); return ret; } @@ -427,8 +444,8 @@ SocketAPM *SocketAPM::accept(uint32_t timeout_ms) return nullptr; } - socklen_t len = sizeof(in_addr); - int newfd = CALL_PREFIX(accept)(fd, (sockaddr *)&in_addr, &len); + socklen_t len = sizeof(last_in_addr); + int newfd = CALL_PREFIX(accept)(fd, (sockaddr *)&last_in_addr, &len); if (newfd == -1) { return nullptr; } diff --git a/libraries/AP_HAL/utility/Socket.h b/libraries/AP_HAL/utility/Socket.h index a7bbf45e5b..480281b5d7 100644 --- a/libraries/AP_HAL/utility/Socket.h +++ b/libraries/AP_HAL/utility/Socket.h @@ -19,22 +19,12 @@ #include #include + #if AP_NETWORKING_SOCKETS_ENABLED #if HAL_OS_SOCKETS -#include -#include -#include -#include -#include -#include -#include -#include -#elif AP_NETWORKING_BACKEND_CHIBIOS -#include -#include -#endif +struct sockaddr_in; class SocketAPM { public: @@ -91,7 +81,13 @@ public: private: bool datagram; - struct sockaddr_in in_addr {}; + struct { + uint16_t sin_family; + uint16_t sin_port; + struct { + uint32_t s_addr; + } sin_addr; + } last_in_addr; bool is_multicast_address(struct sockaddr_in &addr) const; int fd = -1; @@ -104,4 +100,5 @@ private: void make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr); }; +#endif // HAL_OS_SOCKETS #endif // AP_NETWORKING_SOCKETS_ENABLED