From 45a7c37734ccaa6ae9ab8b6b17d20870b37696ba Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 29 Jul 2015 16:46:53 +1000
Subject: [PATCH] HAL_Linux: added bcast flag for udp broadcast

---
 libraries/AP_HAL_Linux/UARTDriver.cpp |  3 ++-
 libraries/AP_HAL_Linux/UDPDevice.cpp  | 28 +++++++++++++++++++++------
 libraries/AP_HAL_Linux/UDPDevice.h    |  4 +++-
 3 files changed, 27 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp
index 7ac69c479b..b8fd30dc49 100644
--- a/libraries/AP_HAL_Linux/UARTDriver.cpp
+++ b/libraries/AP_HAL_Linux/UARTDriver.cpp
@@ -264,7 +264,8 @@ bool LinuxUARTDriver::_serial_start_connection()
  */
 void LinuxUARTDriver::_udp_start_connection(void)
 {
-    _device = new UDPDevice(_ip, _base_port);
+    bool bcast = (_flag && strcmp(_flag, "bcast") == 0);
+    _device = new UDPDevice(_ip, _base_port, bcast);
     _connected = _device->open();
     _device->set_blocking(false);
 
diff --git a/libraries/AP_HAL_Linux/UDPDevice.cpp b/libraries/AP_HAL_Linux/UDPDevice.cpp
index 792808f93e..4e36dd50c9 100644
--- a/libraries/AP_HAL_Linux/UDPDevice.cpp
+++ b/libraries/AP_HAL_Linux/UDPDevice.cpp
@@ -8,15 +8,15 @@
 
 #include "UDPDevice.h"
 
-UDPDevice::UDPDevice(const char *ip, uint16_t port):
+UDPDevice::UDPDevice(const char *ip, uint16_t port, bool bcast):
     _ip(ip),
-    _port(port)
+    _port(port),
+    _bcast(bcast)
 {
 }
 
 UDPDevice::~UDPDevice()
 {
-
 }
 
 ssize_t UDPDevice::write(const uint8_t *buf, uint16_t n)
@@ -24,17 +24,33 @@ ssize_t UDPDevice::write(const uint8_t *buf, uint16_t n)
     if (!socket.pollout(0)) {
         return -1;
     }
-    return socket.send(buf, n);
+    if (_connected) {
+        return socket.send(buf, n);
+    }
+    return socket.sendto(buf, n, _ip, _port);
 }
 
 ssize_t UDPDevice::read(uint8_t *buf, uint16_t n)
 {
-    return socket.recv(buf, n, 0);
+    ssize_t ret = socket.recv(buf, n, 0);
+    if (!_connected && ret > 0) {
+        const char *ip;
+        uint16_t port;
+        socket.last_recv_address(ip, port);
+        _connected = socket.connect(ip, port);
+    }
+    return ret;
 }
 
 bool UDPDevice::open()
 {
-    return socket.connect(_ip, _port);
+    if (_bcast) {
+        // open now, then connect on first received packet
+        socket.set_broadcast();
+        return true;
+    }
+    _connected = socket.connect(_ip, _port);
+    return _connected;
 }
 
 bool UDPDevice::close()
diff --git a/libraries/AP_HAL_Linux/UDPDevice.h b/libraries/AP_HAL_Linux/UDPDevice.h
index ac3a718cf0..368cd391cd 100644
--- a/libraries/AP_HAL_Linux/UDPDevice.h
+++ b/libraries/AP_HAL_Linux/UDPDevice.h
@@ -6,7 +6,7 @@
 
 class UDPDevice: public SerialDevice {
 public:
-    UDPDevice(const char *ip, uint16_t port);
+    UDPDevice(const char *ip, uint16_t port, bool bcast);
     virtual ~UDPDevice();
 
     virtual bool open() override;
@@ -19,6 +19,8 @@ private:
     SocketAPM socket{true};
     const char *_ip;
     uint16_t _port;
+    bool _bcast;
+    bool _connected = false;
 };
 
 #endif