From 72b5cecdb2d425763e4df55475e6237174728faa Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 28 Jun 2016 20:58:31 +1000
Subject: [PATCH] HAL_Linux: added udpin support to HAL_Linux

useful for setting up ardupilot as a mavlink UDP listener
---
 libraries/AP_HAL_Linux/UARTDriver.cpp | 27 +++++++++++++++++++++++++--
 libraries/AP_HAL_Linux/UARTDriver.h   |  2 ++
 libraries/AP_HAL_Linux/UDPDevice.cpp  | 13 +++++++++++--
 libraries/AP_HAL_Linux/UDPDevice.h    |  3 ++-
 4 files changed, 40 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp
index c9a0c87d3d..7c6984b4ba 100644
--- a/libraries/AP_HAL_Linux/UARTDriver.cpp
+++ b/libraries/AP_HAL_Linux/UARTDriver.cpp
@@ -87,6 +87,13 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
             break;
         }
 
+        case DEVICE_UDPIN:
+        {
+            _udpin_start_connection();
+            _flow_control = FLOW_CONTROL_ENABLE;
+            break;
+        }
+        
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
         case DEVICE_QFLIGHT:
         {
@@ -206,7 +213,8 @@ UARTDriver::device_type UARTDriver::_parseDevicePath(const char *arg)
         return DEVICE_QFLIGHT;
 #endif
     } else if (strncmp(arg, "tcp:", 4) != 0 &&
-               strncmp(arg, "udp:", 4) != 0) {
+               strncmp(arg, "udp:", 4) != 0 &&
+               strncmp(arg, "udpin:", 6)) {
         return DEVICE_UNKNOWN;
     }
 
@@ -251,6 +259,8 @@ UARTDriver::device_type UARTDriver::_parseDevicePath(const char *arg)
 
     if (strcmp(protocol, "udp") == 0) {
         type = DEVICE_UDP;
+    } else if (strcmp(protocol, "udpin") == 0) {
+        type = DEVICE_UDPIN;
     } else {
         type = DEVICE_TCP;
     }
@@ -289,7 +299,20 @@ bool UARTDriver::_qflight_start_connection()
 void UARTDriver::_udp_start_connection(void)
 {
     bool bcast = (_flag && strcmp(_flag, "bcast") == 0);
-    _device = new UDPDevice(_ip, _base_port, bcast);
+    _device = new UDPDevice(_ip, _base_port, bcast, false);
+    _connected = _device->open();
+    _device->set_blocking(false);
+
+    /* try to write on MAVLink packet boundaries if possible */
+    _packetise = true;
+}
+
+/*
+  start a UDP connection for the serial port
+ */
+void UARTDriver::_udpin_start_connection(void)
+{
+    _device = new UDPDevice(_ip, _base_port, false, true);
     _connected = _device->open();
     _device->set_blocking(false);
 
diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h
index aeec43a8e3..3b26c4057a 100644
--- a/libraries/AP_HAL_Linux/UARTDriver.h
+++ b/libraries/AP_HAL_Linux/UARTDriver.h
@@ -52,6 +52,7 @@ private:
     void _allocate_buffers(uint16_t rxS, uint16_t txS);
     void _deallocate_buffers();
     void _udp_start_connection(void);
+    void _udpin_start_connection(void);
     void _tcp_start_connection(void);
     bool _serial_start_connection(void);
     bool _qflight_start_connection(void);
@@ -59,6 +60,7 @@ private:
     enum device_type {
         DEVICE_TCP,
         DEVICE_UDP,
+        DEVICE_UDPIN,
         DEVICE_SERIAL,
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
         DEVICE_QFLIGHT,
diff --git a/libraries/AP_HAL_Linux/UDPDevice.cpp b/libraries/AP_HAL_Linux/UDPDevice.cpp
index f5c7539827..81bb1b5345 100644
--- a/libraries/AP_HAL_Linux/UDPDevice.cpp
+++ b/libraries/AP_HAL_Linux/UDPDevice.cpp
@@ -6,10 +6,11 @@
 
 #include <AP_HAL/AP_HAL.h>
 
-UDPDevice::UDPDevice(const char *ip, uint16_t port, bool bcast):
+UDPDevice::UDPDevice(const char *ip, uint16_t port, bool bcast, bool input):
     _ip(ip),
     _port(port),
-    _bcast(bcast)
+    _bcast(bcast),
+    _input(input)
 {
 }
 
@@ -25,6 +26,10 @@ ssize_t UDPDevice::write(const uint8_t *buf, uint16_t n)
     if (_connected) {
         return socket.send(buf, n);
     }
+    if (_input) {
+        // can't send yet
+        return -1;
+    }
     return socket.sendto(buf, n, _ip, _port);
 }
 
@@ -42,6 +47,10 @@ ssize_t UDPDevice::read(uint8_t *buf, uint16_t n)
 
 bool UDPDevice::open()
 {
+    if (_input) {
+        socket.bind(_ip, _port);
+        return true;
+    }
     if (_bcast) {
         // open now, then connect on first received packet
         socket.set_broadcast();
diff --git a/libraries/AP_HAL_Linux/UDPDevice.h b/libraries/AP_HAL_Linux/UDPDevice.h
index dcc7e6f27c..a51e2ddc5b 100644
--- a/libraries/AP_HAL_Linux/UDPDevice.h
+++ b/libraries/AP_HAL_Linux/UDPDevice.h
@@ -5,7 +5,7 @@
 
 class UDPDevice: public SerialDevice {
 public:
-    UDPDevice(const char *ip, uint16_t port, bool bcast);
+    UDPDevice(const char *ip, uint16_t port, bool bcast, bool input);
     virtual ~UDPDevice();
 
     virtual bool open() override;
@@ -19,5 +19,6 @@ private:
     const char *_ip;
     uint16_t _port;
     bool _bcast;
+    bool _input;
     bool _connected = false;
 };