HAL_Linux: implemented TCP server as a single driver

the wait flag just changes startup behaviour. The TCP server should
always be a server with listen and accept. We don't need two drivers

libraries/AP_HAL_Linux/TCPBlockingServerDevice.cpp

@@ -1,138 +0,0 @@
-#include <AP_HAL.h>
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
-
-#include <stdio.h>
-#include <unistd.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <sys/socket.h>
-#include <netdb.h>
-
-#include "TCPBlockingServerDevice.h"
-
-TCPBlockingServerDevice::TCPBlockingServerDevice(const char *ip, uint16_t port):
-    _ip(ip),
-    _port(port)
-{
-}
-
-TCPBlockingServerDevice::~TCPBlockingServerDevice()
-{
-
-}
-
-ssize_t TCPBlockingServerDevice::write(const uint8_t *buf, uint16_t n)
-{
-    return ::send(_net_fd, buf, n, 0);
-}
-
-ssize_t TCPBlockingServerDevice::read(uint8_t *buf, uint16_t n)
-{
-    return ::recv(_net_fd, buf, n, 0);
-}
-
-bool TCPBlockingServerDevice::open()
-{
-    int one=1;
-    struct sockaddr_in sockaddr;
-    int ret;
-    int client_fd = -1;
-    uint8_t portNumber = 0; /* connecto to _port + portNumber */
-
-    memset(&sockaddr,0,sizeof(sockaddr));
-
-#ifdef HAVE_SOCK_SIN_LEN
-    sockaddr.sin_len = sizeof(sockaddr);
-#endif
-    sockaddr.sin_port = htons(_port + portNumber);
-    sockaddr.sin_family = AF_INET;
-
-    if (strcmp(_ip, "*") == 0) {
-        /* bind to all interfaces */
-        sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
-    } else {
-        sockaddr.sin_addr.s_addr = inet_addr(_ip);
-    }
-
-    _listen_fd = socket(AF_INET, SOCK_STREAM, 0);
-    if (_listen_fd == -1) {
-        ::printf("socket failed - %s\n", strerror(errno));
-        exit(1);
-    }
-
-    /* we want to be able to re-use ports quickly */
-    setsockopt(_listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
-
-    ret = bind(_listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
-
-    if (ret == -1) {
-        ::printf("bind failed on port %u - %s\n",
-                 (unsigned)ntohs(sockaddr.sin_port),
-                 strerror(errno));
-        exit(1);
-    }
-
-    ret = listen(_listen_fd, 5);
-    if (ret == -1) {
-        ::printf("listen failed - %s\n", strerror(errno));
-        exit(1);
-    }
-
-    ::printf("Serial port %u on TCP port %u\n", portNumber, 
-             _port + portNumber);
-    ::fflush(stdout);
-
-    ::printf("Waiting for connection ....\n");
-    ::fflush(stdout);
-
-    struct sockaddr_storage client_addr;
-    socklen_t addr_size = sizeof(client_addr);
-
-    client_fd = accept(_listen_fd, (struct sockaddr *) &client_addr, &addr_size);
-
-    if (client_fd == -1) {
-        ::printf("accept() error - %s", strerror(errno));
-        exit(1);
-    }
-
-    struct sockaddr_in *sa4 = (struct sockaddr_in*) &client_addr;
-    printf("%s connected\n", inet_ntoa(sa4->sin_addr) );
-
-    setsockopt(client_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
-    setsockopt(client_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
-
-    /* always run the file descriptor non-blocking, and deal with                                         |
-    *  blocking IO in the higher level calls */
-    fcntl(client_fd, F_SETFL, fcntl(client_fd, F_GETFL, 0) | O_NONBLOCK);
-
-    _net_fd = client_fd;
-
-    return true;
-}
-
-bool TCPBlockingServerDevice::close()
-{
-    if (::close(_listen_fd) < 0) {
-        perror("close");
-        return false;
-    }
-
-    if (::close(_net_fd) < 0) {
-        perror("close");
-        return false;
-    }
-
-    return true;
-}
-
-void TCPBlockingServerDevice::set_blocking(bool blocking)
-{
-}
-
-void TCPBlockingServerDevice::set_speed(uint32_t speed)
-{
-
-}
-
-#endif

libraries/AP_HAL_Linux/TCPBlockingServerDevice.h

@@ -1,25 +0,0 @@
-#ifndef __AP_HAL_LINUX_TCPSERVERDEVICE_H__
-#define __AP_HAL_LINUX_TCPSERVERDEVICE_H__
-
-#include "SerialDevice.h"
-#include "../AP_HAL/utility/Socket.h"
-
-class TCPBlockingServerDevice: public SerialDevice {
-public:
-    TCPBlockingServerDevice(const char *ip, uint16_t port);
-    virtual ~TCPBlockingServerDevice();
-
-    virtual bool open() override;
-    virtual bool close() override;
-    virtual void set_blocking(bool blocking) override;
-    virtual void set_speed(uint32_t speed) override;
-    virtual ssize_t write(const uint8_t *buf, uint16_t n) override;
-    virtual ssize_t read(uint8_t *buf, uint16_t n) override;
-private:
-    const char *_ip;
-    uint16_t _port;
-    int _net_fd = -1;
-    int _listen_fd = -1;
-};
-
-#endif

libraries/AP_HAL_Linux/TCPServerDevice.cpp

@@ -9,33 +9,68 @@
 
 #include "TCPServerDevice.h"
 
-TCPServerDevice::TCPServerDevice(const char *ip, uint16_t port):
+TCPServerDevice::TCPServerDevice(const char *ip, uint16_t port, bool wait):
     _ip(ip),
-    _port(port)
+    _port(port),
+    _wait(wait)
 {
 }
 
 TCPServerDevice::~TCPServerDevice()
 {
-
+    if (sock != NULL) {
+        delete sock;
+        sock = NULL;
+    }
 }
 
 ssize_t TCPServerDevice::write(const uint8_t *buf, uint16_t n)
 {
-    return listener.send(buf, n);
+    if (sock == NULL) {
+        return -1;
+    }
+    return sock->send(buf, n);
 }
 
+/*
+  when we try to read we accept new connections if one isn't already
+  established
+ */
 ssize_t TCPServerDevice::read(uint8_t *buf, uint16_t n)
 {
-    return listener.recv(buf, n, 1);
+    if (sock == NULL) {
+        sock = listener.accept(0);
+        if (sock != NULL) {
+            sock->set_blocking(_blocking);
+        }
+    }
+    if (sock == NULL) {
+        return -1;
+    }
+    ssize_t ret = sock->recv(buf, n, 1);
+    if (ret == 0) {
+        // EOF, go back to waiting for a new connection
+        delete sock;
+        sock = NULL;
+        return -1;
+    }
+    return ret;
 }
 
 bool TCPServerDevice::open()
 {
     listener.reuseaddress();
 
-    if (!listener.connect(_ip, _port)) {
+    if (!listener.bind(_ip, _port)) {
-        ::printf("connect failed on %s port %u - %s\n",
+        ::printf("bind failed on %s port %u - %s\n",
+                 _ip,
+                 _port,
+                 strerror(errno));
+        ::exit(1);
+    }
+
+    if (!listener.listen(1)) {
+        ::printf("listen failed on %s port %u - %s\n",
                  _ip,
                  _port,
                  strerror(errno));
@@ -44,22 +79,38 @@ bool TCPServerDevice::open()
 
     listener.set_blocking(false);
 
+    if (_wait) {
+        ::printf("Waiting for connection on %s:%u ....\n",
+                 _ip, (unsigned)_port);
+        ::fflush(stdout);
+        while (sock == NULL) {
+            sock = listener.accept(1000);
+        }
+        sock->set_blocking(_blocking);
+        ::printf("connected\n");
+        ::fflush(stdout);
+    }
+
     return true;
 }
 
 bool TCPServerDevice::close()
 {
+    if (sock != NULL) {
+        delete sock;
+        sock = NULL;
+    }
     return true;
 }
 
 void TCPServerDevice::set_blocking(bool blocking)
 {
-    listener.set_blocking(blocking);
+    _blocking = blocking;
+    listener.set_blocking(_blocking);
 }
 
 void TCPServerDevice::set_speed(uint32_t speed)
 {
-
 }
 
 #endif

libraries/AP_HAL_Linux/TCPServerDevice.h

@@ -6,7 +6,7 @@
 
 class TCPServerDevice: public SerialDevice {
 public:
-    TCPServerDevice(const char *ip, uint16_t port);
+    TCPServerDevice(const char *ip, uint16_t port, bool wait);
     virtual ~TCPServerDevice();
 
     virtual bool open() override;
@@ -15,10 +15,14 @@ public:
     virtual void set_speed(uint32_t speed) override;
     virtual ssize_t write(const uint8_t *buf, uint16_t n) override;
     virtual ssize_t read(uint8_t *buf, uint16_t n) override;
+
 private:
     SocketAPM listener{false};
+    SocketAPM *sock = NULL;
     const char *_ip;
     uint16_t _port;
+    bool _wait;
+    bool _blocking = false;
 };
 
 #endif

libraries/AP_HAL_Linux/UARTDriver.cpp

@@ -28,7 +28,6 @@
 #include "UDPDevice.h"
 #include "ConsoleDevice.h"
 #include "TCPServerDevice.h"
-#include "TCPBlockingServerDevice.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -274,15 +273,8 @@ void LinuxUARTDriver::_udp_start_connection(void)
 
 void LinuxUARTDriver::_tcp_start_connection(void)
 {
-    if (_flag != NULL) {
+    bool wait = (_flag && strcmp(_flag, "wait") == 0);
-        if (!strcmp(_flag, "wait")) {    
+    _device = new TCPServerDevice(_ip, _base_port, wait);
-            _device = new TCPBlockingServerDevice(_ip, _base_port);
-        } else {
-            _device = new TCPServerDevice(_ip, _base_port);
-        }
-    } else {
-        _device = new TCPServerDevice(_ip, _base_port);
-    }
 
     if (_device->open()) {
         _connected = true;