HAL_SITL: added support for multicast UDP connections

this allows several copies of SITL to see each other on the same
network, emulating a mesh network, which is useful for swarm testing

libraries/AP_HAL_SITL/UARTDriver.cpp

@@ -74,6 +74,10 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
              tcp:5760:wait    // tcp listen on port 5760
              tcp:0:wait       // tcp listen on use base_port + 0
              tcpclient:192.168.2.15:5762
+             udpclient:127.0.0.1
+             udpclient:127.0.0.1:14550
+             mcast:
+             mcast:239.255.145.50:14550
              uart:/dev/ttyUSB0:57600
              sim:ParticleSensor_SDS021:
          */
@@ -105,10 +109,21 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
                 _fd = _sitlState->sim_fd(args1, args2);
             }
         } else if (strcmp(devtype, "udpclient") == 0) {
+            // udp client connection
             const char *ip = args1;
             uint16_t port = args2?atoi(args2):14550;
+            if (!_connected) {
                 ::printf("UDP connection %s:%u\n", ip, port);
                 _udp_start_client(ip, port);
+            }
+        } else if (strcmp(devtype, "mcast") == 0) {
+            // udp multicast connection
+            const char *ip = args1 && *args1?args1:mcast_ip_default;
+            uint16_t port = args2?atoi(args2):mcast_port_default;
+            if (!_connected) {
+                ::printf("UDP multicast connection %s:%u\n", ip, port);
+                _udp_start_multicast(ip, port);
+            }
         } else {
             AP_HAL::panic("Invalid device path: %s", path);
         }
@@ -404,6 +419,71 @@ void UARTDriver::_udp_start_client(const char *address, uint16_t port)
     _connected = true;
 }
 
+/*
+  start a UDP multicast connection
+ */
+void UARTDriver::_udp_start_multicast(const char *address, uint16_t port)
+{
+    if (_connected) {
+        return;
+    }
+
+    // establish the listening port
+    struct sockaddr_in sockaddr;
+    int ret;
+
+    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);
+
+    _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);
+    }
+
+    // close on exec, to allow reboot
+    fcntl(_mc_fd, F_SETFD, FD_CLOEXEC);
+
+    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(address);
+    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));
+        exit(1);
+    }
+
+    // now start the outgoing connection as an ordinary UDP connection
+    _udp_start_client(address, port);
+}
+
 
 /*
   start a UART connection for the serial port
@@ -574,7 +654,33 @@ void UARTDriver::_timer_tick(void)
     
     char buf[space];
     ssize_t nread = 0;
-    if (!_use_send_recv) {
+    if (_mc_fd >= 0) {
+        if (_select_check(_mc_fd)) {
+            struct sockaddr_in from;
+            socklen_t fromlen = sizeof(from);
+            nread = recvfrom(_mc_fd, buf, space, MSG_DONTWAIT, (struct sockaddr *)&from, &fromlen);
+            uint16_t port = ntohs(from.sin_port);
+            if (_mc_myport == 0) {
+                // get our own address, so we can recognise packets from ourself
+                struct sockaddr_in myaddr;
+                socklen_t myaddrlen;
+                if (getsockname(_fd, (struct sockaddr *)&myaddr, &myaddrlen) == 0) {
+                    _mc_myport = ntohs(myaddr.sin_port);
+                }
+            }
+            if (_mc_myport == port) {
+                // assume this is a packet from ourselves. This is not
+                // entirely accurate, as it could be a packet from
+                // another machine that has assigned the same port,
+                // unfortunately we don't have a better way to detect
+                // packets from ourselves
+                nread = 0;
+            }
+        }
+    } else if (!_use_send_recv) {
+        if (!_select_check(_fd)) {
+            return;
+        }
         int fd = _console?0:_fd;
         nread = ::read(fd, buf, space);
         if (nread == -1 && errno != EAGAIN && _uart_path) {
@@ -582,20 +688,17 @@ void UARTDriver::_timer_tick(void)
             _fd = -1;
             _connected = false;
         }
-    } else {
-        if (_select_check(_fd)) {
+    } else if (_select_check(_fd)) {
         nread = recv(_fd, buf, space, MSG_DONTWAIT);
         if (nread <= 0 && !_is_udp) {
             // the socket has reached EOF
             close(_fd);
+            _fd = -1;
             _connected = false;
             fprintf(stdout, "Closed connection on serial port %u\n", _portNumber);
             fflush(stdout);
             return;
         }
-        } else {
-            nread = 0;
-        }
     }
     if (nread > 0) {
         _readbuffer.write((uint8_t *)buf, nread);

libraries/AP_HAL_SITL/UARTDriver.h

@@ -18,6 +18,7 @@ public:
         _sitlState = sitlState;
 
         _fd = -1;
+        _mc_fd = -1;
         _listen_fd = -1;
     }
 
@@ -57,6 +58,9 @@ public:
     // file descriptor, exposed so SITL_State::loop_hook() can use it
     int _fd;
 
+    // file descriptor for reading multicast packets
+    int _mc_fd;
+
     bool _unbuffered_writes;
 
     enum flow_control get_flow_control(void) override { return FLOW_CONTROL_ENABLE; }
@@ -93,6 +97,10 @@ private:
     ByteBuffer _readbuffer{16384};
     ByteBuffer _writebuffer{16384};
 
+    // default multicast IP and port
+    const char *mcast_ip_default = "239.255.145.50";
+    const uint16_t mcast_port_default = 14550;
+
     const char *_uart_path;
     uint32_t _uart_baudrate;
 
@@ -104,6 +112,7 @@ private:
     void _check_reconnect();
     void _tcp_start_client(const char *address, uint16_t port);
     void _udp_start_client(const char *address, uint16_t port);
+    void _udp_start_multicast(const char *address, uint16_t port);
     void _check_connection(void);
     static bool _select_check(int );
     static void _set_nonblocking(int );
@@ -113,6 +122,7 @@ private:
     uint64_t _receive_timestamp;
     bool _is_udp;
     bool _packetise;
+    uint16_t _mc_myport;
 };
 
 #endif