diff --git a/libraries/AP_HAL/utility/Socket.cpp b/libraries/AP_HAL/utility/Socket.cpp new file mode 100644 index 0000000000..96c13c6bac --- /dev/null +++ b/libraries/AP_HAL/utility/Socket.cpp @@ -0,0 +1,135 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simple socket handling class for systems with BSD socket API + */ + +#include +#if HAL_OS_SOCKETS + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Socket.h" + +/* + constructor + */ +SocketAPM::SocketAPM(bool _datagram) : +datagram(_datagram) +{ + fd = socket(AF_INET, datagram?SOCK_DGRAM:SOCK_STREAM, 0); +} + +/* + connect the socket + */ +bool SocketAPM::connect(const char *address, uint16_t port) +{ + struct sockaddr_in sockaddr; + memset(&sockaddr, 0, sizeof(sockaddr)); + +#ifdef HAVE_SOCK_SIN_LEN + sockaddr.sin_len = sizeof(sockaddr); +#endif + sockaddr.sin_port = htons(port); + sockaddr.sin_family = AF_INET; + sockaddr.sin_addr.s_addr = inet_addr(address); + + if (::connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) { + return false; + } + return true; +} + +/* + bind the socket + */ +bool SocketAPM::bind(const char *address, uint16_t port) +{ + struct sockaddr_in sockaddr; + memset(&sockaddr, 0, sizeof(sockaddr)); + +#ifdef HAVE_SOCK_SIN_LEN + sockaddr.sin_len = sizeof(sockaddr); +#endif + sockaddr.sin_port = htons(port); + sockaddr.sin_family = AF_INET; + sockaddr.sin_addr.s_addr = inet_addr(address); + + if (::bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) { + return false; + } + return true; +} + + +/* + set SO_REUSEADDR + */ +void SocketAPM::reuseaddress(void) +{ + int one = 1; + setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); +} + +/* + set blocking state + */ +void SocketAPM::set_blocking(bool blocking) +{ + if (blocking) { + fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) & ~O_NONBLOCK); + } else { + fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK); + } +} + +/* + send some data + */ +ssize_t SocketAPM::send(void *buf, size_t size) +{ + return ::send(fd, buf, size, 0); +} + +/* + receive some data + */ +ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms) +{ + fd_set fds; + struct timeval tv; + + FD_ZERO(&fds); + FD_SET(fd, &fds); + + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000UL; + + if (select(fd+1, &fds, NULL, NULL, &tv) != 1) { + return -1; + } + + return ::recv(fd, buf, size, 0); +} + +#endif // HAL_OS_SOCKETS diff --git a/libraries/AP_HAL/utility/Socket.h b/libraries/AP_HAL/utility/Socket.h new file mode 100644 index 0000000000..3c332f5da9 --- /dev/null +++ b/libraries/AP_HAL/utility/Socket.h @@ -0,0 +1,38 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simple socket handling class for systems with BSD socket API + */ + +#include +#if HAL_OS_SOCKETS + +class SocketAPM { +public: + SocketAPM(bool _datagram); + bool connect(const char *address, uint16_t port); + bool bind(const char *address, uint16_t port); + void reuseaddress(); + void set_blocking(bool blocking); + + ssize_t send(void *pkt, size_t size); + ssize_t recv(void *pkt, size_t size, uint32_t timeout_ms); + +private: + bool datagram; + int fd; +}; + +#endif // HAL_OS_SOCKETS