From 58cf4f65a90f8bcb26f7cdb5c599c07a5779e12d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 23 Nov 2023 16:56:12 +1100 Subject: [PATCH] HAL_SITL: use SocketAPM for CAN multicast and SITL_Periph state --- libraries/AP_HAL_SITL/CAN_Multicast.cpp | 94 +-------------------- libraries/AP_HAL_SITL/CAN_Multicast.h | 5 +- libraries/AP_HAL_SITL/SITL_Periph_State.cpp | 91 ++++---------------- libraries/AP_HAL_SITL/SITL_Periph_State.h | 5 +- 4 files changed, 22 insertions(+), 173 deletions(-) diff --git a/libraries/AP_HAL_SITL/CAN_Multicast.cpp b/libraries/AP_HAL_SITL/CAN_Multicast.cpp index b83642f7ef..fe84347324 100644 --- a/libraries/AP_HAL_SITL/CAN_Multicast.cpp +++ b/libraries/AP_HAL_SITL/CAN_Multicast.cpp @@ -38,83 +38,9 @@ bool CAN_Multicast::init(uint8_t instance) { // setup incoming multicast socket char address[] = MCAST_ADDRESS_BASE; - struct sockaddr_in sockaddr {}; - struct ip_mreq mreq {}; - int one = 1; - int ret; -#ifdef HAVE_SOCK_SIN_LEN - sockaddr.sin_len = sizeof(sockaddr); -#endif address[strlen(address)-1] = '0' + instance; - - sockaddr.sin_port = htons(MCAST_PORT); - sockaddr.sin_family = AF_INET; - sockaddr.sin_addr.s_addr = inet_addr(address); - - fd_in = socket(AF_INET, SOCK_DGRAM, 0); - if (fd_in == -1) { - goto fail; - } - ret = fcntl(fd_in, F_SETFD, FD_CLOEXEC); - if (ret == -1) { - goto fail; - } - if (setsockopt(fd_in, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) == -1) { - goto fail; - } - - // close on exec, to allow reboot - fcntl(fd_in, F_SETFD, FD_CLOEXEC); - -#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.sin_addr.s_addr = htonl(INADDR_ANY); -#endif - - ret = bind(fd_in, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); - if (ret == -1) { - goto fail; - } - - mreq.imr_multiaddr.s_addr = inet_addr(address); - mreq.imr_interface.s_addr = htonl(INADDR_ANY); - - ret = setsockopt(fd_in, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); - if (ret == -1) { - goto fail; - } - - // setup outgoing socket - fd_out = socket(AF_INET, SOCK_DGRAM, 0); - if (fd_out == -1) { - goto fail; - } - ret = fcntl(fd_out, F_SETFD, FD_CLOEXEC); - if (ret == -1) { - goto fail; - } - - ret = connect(fd_out, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); - if (ret == -1) { - goto fail; - } - - return true; - -fail: - if (fd_in != -1) { - (void)close(fd_in); - fd_in = -1; - } - if (fd_out != -1) { - (void)close(fd_out); - fd_out = -1; - } - return false; + return sock.connect(address, MCAST_PORT); } /* @@ -135,7 +61,7 @@ bool CAN_Multicast::send(const AP_HAL::CANFrame &frame) memcpy(pkt.data, frame.data, data_length); pkt.crc = crc16_ccitt((uint8_t*)&pkt.flags, data_length+6, 0xFFFFU); - return ::send(fd_out, (void*)&pkt, data_length+10, 0) == data_length+10; + return sock.send((void*)&pkt, data_length+10) == data_length+10; } /* @@ -144,9 +70,7 @@ bool CAN_Multicast::send(const AP_HAL::CANFrame &frame) bool CAN_Multicast::receive(AP_HAL::CANFrame &frame) { struct mcast_pkt pkt; - struct sockaddr_in src_addr; - socklen_t src_len = sizeof(src_addr); - ssize_t ret = ::recvfrom(fd_in, (void*)&pkt, sizeof(pkt), MSG_DONTWAIT, (struct sockaddr *)&src_addr, &src_len); + ssize_t ret = sock.recv((void*)&pkt, sizeof(pkt), 0); if (ret < 10) { return false; } @@ -157,18 +81,6 @@ bool CAN_Multicast::receive(AP_HAL::CANFrame &frame) return false; } - // ensure it isn't a packet we sent - struct sockaddr_in send_addr; - socklen_t send_len = sizeof(send_addr); - if (getsockname(fd_out, (struct sockaddr *)&send_addr, &send_len) != 0) { - return false; - } - if (src_addr.sin_port == send_addr.sin_port && - src_addr.sin_family == send_addr.sin_family && - src_addr.sin_addr.s_addr == send_addr.sin_addr.s_addr) { - return false; - } - // run constructor to initialise new(&frame) AP_HAL::CANFrame(pkt.message_id, pkt.data, ret-10, (pkt.flags & MCAST_FLAG_CANFD) != 0); diff --git a/libraries/AP_HAL_SITL/CAN_Multicast.h b/libraries/AP_HAL_SITL/CAN_Multicast.h index be8144cb65..d3725458e3 100644 --- a/libraries/AP_HAL_SITL/CAN_Multicast.h +++ b/libraries/AP_HAL_SITL/CAN_Multicast.h @@ -13,12 +13,11 @@ public: bool send(const AP_HAL::CANFrame &frame) override; bool receive(AP_HAL::CANFrame &frame) override; int get_read_fd(void) const override { - return fd_in; + return sock.get_read_fd(); } private: - int fd_in = -1; - int fd_out = -1; + SocketAPM sock{true}; }; #endif // HAL_NUM_CAN_IFACES diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp index f72214aa46..27f1b063c8 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp @@ -138,59 +138,11 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) */ void SimMCast::multicast_open(void) { - struct sockaddr_in sockaddr {}; - int ret; - -#ifdef HAVE_SOCK_SIN_LEN - sockaddr.sin_len = sizeof(sockaddr); -#endif - sockaddr.sin_port = htons(SITL_MCAST_PORT); - sockaddr.sin_family = AF_INET; - sockaddr.sin_addr.s_addr = inet_addr(SITL_MCAST_IP); - - mc_fd = socket(AF_INET, SOCK_DGRAM, 0); - if (mc_fd == -1) { - fprintf(stderr, "socket failed - %s\n", strerror(errno)); - exit(1); - } - ret = fcntl(mc_fd, F_SETFD, FD_CLOEXEC); - if (ret == -1) { - fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno)); - exit(1); - } - int one = 1; - if (setsockopt(mc_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) == -1) { - fprintf(stderr, "setsockopt failed: %s\n", strerror(errno)); - exit(1); - } - -#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.sin_addr.s_addr = htonl(INADDR_ANY); -#endif - - ret = bind(mc_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); - if (ret == -1) { - fprintf(stderr, "multicast bind failed on port %u - %s\n", - (unsigned)ntohs(sockaddr.sin_port), - strerror(errno)); - exit(1); - } - - struct ip_mreq mreq {}; - mreq.imr_multiaddr.s_addr = inet_addr(SITL_MCAST_IP); - mreq.imr_interface.s_addr = htonl(INADDR_ANY); - - ret = setsockopt(mc_fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); - if (ret == -1) { - fprintf(stderr, "multicast membership add failed on port %u - %s\n", - (unsigned)ntohs(sockaddr.sin_port), - strerror(errno)); + if (!sock.connect(SITL_MCAST_IP, SITL_MCAST_PORT)) { + fprintf(stderr, "multicast socket failed - %s\n", strerror(errno)); exit(1); } + servo_sock.set_blocking(false); ::printf("multicast receiver initialised\n"); } @@ -199,29 +151,17 @@ void SimMCast::multicast_open(void) */ void SimMCast::servo_fd_open(void) { - servo_fd = socket(AF_INET, SOCK_DGRAM, 0); - if (servo_fd == -1) { - fprintf(stderr, "socket failed - %s\n", strerror(errno)); - exit(1); - } - int ret = fcntl(servo_fd, F_SETFD, FD_CLOEXEC); - if (ret == -1) { - fprintf(stderr, "fcntl failed on setting FD_CLOEXEC - %s\n", strerror(errno)); - exit(1); - } - int one = 1; - if (setsockopt(servo_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) == -1) { - fprintf(stderr, "setsockopt failed: %s\n", strerror(errno)); - exit(1); - } - - in_addr.sin_port = htons(SITL_SERVO_PORT); - - ret = connect(servo_fd, (struct sockaddr *)&in_addr, sizeof(in_addr)); - if (ret == -1) { - fprintf(stderr, "multicast servo connect failed\n"); + const char *in_addr = nullptr; + uint16_t port; + sock.last_recv_address(in_addr, port); + if (in_addr == nullptr) { + return; + } + if (!servo_sock.connect(in_addr, SITL_SERVO_PORT)) { + fprintf(stderr, "servo socket failed - %s\n", strerror(errno)); exit(1); } + servo_sock.set_blocking(false); } /* @@ -241,7 +181,7 @@ void SimMCast::servo_send(void) for (uint8_t i=0; istate.timestamp_us == 0) { @@ -278,7 +217,7 @@ void SimMCast::multicast_read(void) } hal.scheduler->stop_clock(_sitl->state.timestamp_us + base_time_us); HALSITL::Scheduler::timer_event(); - if (servo_fd == -1) { + if (!servo_sock.is_connected()) { servo_fd_open(); } else { servo_send(); diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.h b/libraries/AP_HAL_SITL/SITL_Periph_State.h index 7a9d3ec757..b7e21d1491 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.h +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.h @@ -30,9 +30,8 @@ public: void update(const struct sitl_input &input) override; private: - int mc_fd = -1; - int servo_fd = -1; - struct sockaddr_in in_addr; + SocketAPM sock{true}; + SocketAPM servo_sock{true}; // offset between multicast timestamp and local timestamp uint64_t base_time_us;