Skip to content

Commit

Permalink
AP_HAL: fixed build
Browse files Browse the repository at this point in the history
  • Loading branch information
tridge authored and magicrub committed Nov 13, 2023
1 parent 4dd658d commit 519256a
Show file tree
Hide file tree
Showing 2 changed files with 16 additions and 27 deletions.
38 changes: 15 additions & 23 deletions libraries/AP_HAL/utility/Socket.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,20 +23,6 @@
#include "Socket.h"
#include <errno.h>

#if AP_NETWORKING_BACKEND_CHIBIOS
#include <AP_Networking/AP_Networking.h>
#include <lwip/sockets.h>

// not defined/implememnted in LwIP.
#ifndef F_SETFD
#define F_SETFD 2
#endif

/* For F_[GET|SET]FD. */
#define FD_CLOEXEC 1 /* Actually anything with low bit set goes */

#endif

/*
constructor
*/
Expand All @@ -49,7 +35,9 @@ SocketAPM::SocketAPM(bool _datagram, int _fd) :
datagram(_datagram),
fd(_fd)
{
#ifdef FD_CLOEXEC
fcntl(fd, F_SETFD, FD_CLOEXEC);
#endif
if (!datagram) {
int one = 1;
setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
Expand All @@ -60,7 +48,7 @@ SocketAPM::~SocketAPM()
{
if (fd != -1) {
#if AP_NETWORKING_BACKEND_CHIBIOS
lwip_close(fd);
::lwip_close(fd);
#else
::close(fd);
#endif
Expand Down Expand Up @@ -89,7 +77,7 @@ bool SocketAPM::connect(const char *address, uint16_t port)
make_sockaddr(address, port, sockaddr);

#if AP_NETWORKING_BACKEND_CHIBIOS
if (lwip_connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
if (::lwip_connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
#else
if (::connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
#endif
Expand All @@ -109,7 +97,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));
int ret = ::lwip_connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
#else
int ret = ::connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
#endif
Expand Down Expand Up @@ -141,7 +129,7 @@ bool SocketAPM::bind(const char *address, uint16_t port)
make_sockaddr(address, port, sockaddr);

#if AP_NETWORKING_BACKEND_CHIBIOS
if (lwip_bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
if (::lwip_bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
#else
if (::bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) {
#endif
Expand Down Expand Up @@ -179,7 +167,11 @@ bool SocketAPM::set_blocking(bool blocking) const
*/
bool SocketAPM::set_cloexec() const
{
#ifdef FD_CLOEXEC
return (fcntl(fd, F_SETFD, FD_CLOEXEC) != -1);
#else
return false;
#endif
}

/*
Expand All @@ -188,7 +180,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);
return ::lwip_send(fd, buf, size, 0);
#else
return ::send(fd, buf, size, 0);
#endif
Expand All @@ -202,7 +194,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));
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
Expand All @@ -218,7 +210,7 @@ ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms)
}
socklen_t len = sizeof(in_addr);
#if AP_NETWORKING_BACKEND_CHIBIOS
return lwip_recvfrom(fd, buf, size, MSG_DONTWAIT, (sockaddr *)&in_addr, &len);
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
Expand Down Expand Up @@ -286,7 +278,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;
return ::lwip_listen(fd, (int)backlog) == 0;
#else
return ::listen(fd, (int)backlog) == 0;
#endif
Expand All @@ -303,7 +295,7 @@ SocketAPM *SocketAPM::accept(uint32_t timeout_ms)
}

#if AP_NETWORKING_BACKEND_CHIBIOS
int newfd = lwip_accept(fd, nullptr, nullptr);
int newfd = ::lwip_accept(fd, nullptr, nullptr);
#else
int newfd = ::accept(fd, nullptr, nullptr);
#endif
Expand Down
5 changes: 1 addition & 4 deletions libraries/AP_HAL/utility/Socket.h
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
#include <sys/select.h>
#elif AP_NETWORKING_BACKEND_CHIBIOS
#include <AP_Networking/AP_Networking_ChibiOS.h>
#include <lwip/sockets.h>
#endif

class SocketAPM {
Expand Down Expand Up @@ -71,11 +72,7 @@ class SocketAPM {

private:
bool datagram;
#if AP_NETWORKING_BACKEND_CHIBIOS
typedef struct sockaddr_in in_addr;
#else
struct sockaddr_in in_addr {};
#endif

int fd = -1;

Expand Down

0 comments on commit 519256a

Please sign in to comment.