HAL_Linux: UART-like TCP sockets impl. on AP_HAL_Linux

libraries/AP_HAL_Linux/HAL_Linux_Class.cpp

@@ -46,6 +46,16 @@ HAL_Linux::HAL_Linux() :
         &utilInstance)
 {}
 
+void _usage(void)
+{
+    printf("Usage: -A uartAPath -B uartBPath -C uartCPath\n");
+    printf("Options:\n");
+    printf("\t-serial:          -A /dev/ttyO4\n");
+    printf("\t                  -B /dev/ttyS1\n");    
+    printf("\t-tcp:             -C tcp:192.168.2.15:1243:wait\n");
+    printf("\t                  -A tcp:11.0.0.2:5678\n");    
+}
+
 void HAL_Linux::init(int argc,char* const argv[]) const 
 {
     int opt;
@@ -64,7 +74,7 @@ void HAL_Linux::init(int argc,char* const argv[]) const
             uartCDriver.set_device_path(optarg);
             break;
         case 'h':
-            printf("Usage: -A uartAPath -B uartBPath -C uartCPath\n");
+            _usage();
             exit(0);
         default:
             printf("Unknown option '%c'\n", (char)opt);

libraries/AP_HAL_Linux/UARTDriver.cpp

@@ -15,11 +15,21 @@
 #include <poll.h>
 #include <assert.h>
 #include <sys/ioctl.h>
+#include <sys/types.h> 
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <string.h>
+#include <arpa/inet.h>
 
 extern const AP_HAL::HAL& hal;
 
 using namespace Linux;
 
+#define DEVICE_TCP 0
+#define DEVICE_SERIAL 1
+#define DEVICE_UNKNOWN 99
+
 LinuxUARTDriver::LinuxUARTDriver(bool default_console) :
     device_path(NULL),
     _rd_fd(-1),
@@ -35,7 +45,7 @@ LinuxUARTDriver::LinuxUARTDriver(bool default_console) :
 /*
   set the tty device to use for this UART
  */
-void LinuxUARTDriver::set_device_path(const char *path)
+void LinuxUARTDriver::set_device_path(char *path)
 {
     device_path = path;
 }
@@ -61,43 +71,85 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
         if (device_path == NULL) {
             return;
         }
-        uint8_t retries = 0;
+        
-        while (retries < 5) {
+        switch (_parseDevicePath(device_path)){        
-            _rd_fd = open(device_path, O_RDWR);
+        case DEVICE_TCP:
-            if (_rd_fd != -1) {
+            {
+                _connected = false;
+                if (_flag != NULL){
+                    _tcp_start_connection(true);    
+                } else {
+                    _tcp_start_connection(false);    
+                }
+                
+                if (_connected) {
+                    if (rxS < 1024) {
+                        rxS = 1024;
+                    }
+                    if (txS < 1024) {
+                        txS = 1024;
+                    }           
+                } else {                
+                    printf("LinuxUARTDriver TCP connection not stablished\n");
+                    exit(1);
+                }
                 break;
             }   
-            // sleep a bit and retry. There seems to be a NuttX bug
+            case DEVICE_SERIAL:            
-            // that can cause ttyACM0 to not be available immediately,
+            {
-            // but a small delay can fix it
+                uint8_t retries = 0;
-            hal.scheduler->delay(100);
+                while (retries < 5) {
-            retries++;
+                    _rd_fd = open(device_path, O_RDWR);
-        }
+                    if (_rd_fd != -1) {
-        _wr_fd = _rd_fd;
+                        break;
-        if (_rd_fd == -1) {
+                    }
-            fprintf(stdout, "Failed to open UART device %s - %s\n",
+                    // sleep a bit and retry. There seems to be a NuttX bug
-                    device_path, strerror(errno));
+                    // that can cause ttyACM0 to not be available immediately,
-            return;
+                    // but a small delay can fix it
-        }
+                    hal.scheduler->delay(100);
-        if (retries != 0) {
+                    retries++;
-            fprintf(stdout, "WARNING: took %u retries to open UART %s\n", 
+                }
-                    (unsigned)retries, device_path);
+                _wr_fd = _rd_fd;
-            return;
+                if (_rd_fd == -1) {
-        }
+                    fprintf(stdout, "Failed to open UART device %s - %s\n",
+                            device_path, strerror(errno));
+                    return;
+                }
+                if (retries != 0) {
+                    fprintf(stdout, "WARNING: took %u retries to open UART %s\n", 
+                            (unsigned)retries, device_path);
+                    return;
+                }
 
-        // always run the file descriptor non-blocking, and deal with
+                // always run the file descriptor non-blocking, and deal with
-        // blocking IO in the higher level calls
+                // blocking IO in the higher level calls
-        fcntl(_rd_fd, F_SETFL, fcntl(_rd_fd, F_GETFL, 0) | O_NONBLOCK);
+                fcntl(_rd_fd, F_SETFL, fcntl(_rd_fd, F_GETFL, 0) | O_NONBLOCK);
                 
-        if (rxS < 1024) {
+                if (rxS < 1024) {
-            rxS = 1024;
+                    rxS = 1024;
-        }
+                }
 
-        // we have enough memory to have a larger transmit buffer for
+                // we have enough memory to have a larger transmit buffer for
-        // all ports. This means we don't get delays while waiting to
+                // all ports. This means we don't get delays while waiting to
-        // write GPS config packets
+                // write GPS config packets
-        if (txS < 1024) {
+                if (txS < 1024) {
-            txS = 1024;
+                    txS = 1024;
+                }
+                break;
+            }
+            default:
+            {
+                // Notify that the option is not valid and select standart input and output
+                printf("LinuxUARTDriver parsing failed, using default\n");
+
+                _rd_fd = 0;
+                _wr_fd = 1;
+                rxS = 512;
+                txS = 512;
+                fcntl(_rd_fd, F_SETFL, fcntl(_rd_fd, F_GETFL, 0) | O_NONBLOCK);
+                fcntl(_wr_fd, F_SETFL, fcntl(_wr_fd, F_GETFL, 0) | O_NONBLOCK);
+                break;
+            }
         }
     }
 
@@ -145,12 +197,129 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
     }
 }
 
+/*
+    Device path accepts the following syntaxes:
+        - /dev/ttyO1
+        - tcp:192.168.2.15:1243:wait
+*/
+int LinuxUARTDriver::_parseDevicePath(char* arg)
+{
+    const char *serial_string = "/dev/tty";
+    const char *tcp_string = "tcp";
+    _flag = NULL; // init flag
+
+    if(strstr(arg, tcp_string) != NULL){        
+        // Parse the TCP string        
+        char *protocol, *ip, *port, *flag;
+        protocol = strtok ( arg, ":" );
+        ip = strtok ( NULL, ":" );
+        port = strtok ( NULL, ":" );
+        flag = strtok ( NULL, ":" );        
+        
+        _base_port = (uint16_t) atoi(port);
+        _ip = ip;        
+        _flag = flag;
+        return DEVICE_TCP;
+    } else if (strstr(arg, serial_string) != NULL){
+        return DEVICE_SERIAL;
+    } else {
+        return DEVICE_UNKNOWN;
+    }    
+}
+
+/*
+  start a TCP connection for the serial port. If wait_for_connection
+  is true then block until a client connects
+ */
+void LinuxUARTDriver::_tcp_start_connection(bool wait_for_connection)
+{
+    int one=1;
+    struct sockaddr_in sockaddr;
+    int ret;    
+    int listen_fd = -1;  // socket we are listening on    
+    int net_fd = -1; // network file descriptor, will be linked to wr_fd and rd_fd
+    uint8_t portNumber = 1;
+
+    // if (_console) {
+    //         // hack for console access
+    //         connected = true;
+    //         listen_fd = -1;
+    //         fd = 1;
+    //         return;
+    // }
+    
+    if (net_fd != -1) {
+        close(net_fd);
+    }
+
+    if (listen_fd == -1) {
+        memset(&sockaddr,0,sizeof(sockaddr));
+
+#ifdef HAVE_SOCK_SIN_LEN
+        sockaddr.sin_len = sizeof(sockaddr);
+#endif
+        sockaddr.sin_port = htons(_base_port + portNumber);
+        sockaddr.sin_family = AF_INET;
+        // sockaddr.sin_addr.s_addr = inet_addr(_base_ip);
+        
+        // Bind to all interfaces
+        sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);
+
+        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));
+
+        printf("bind port %u for %u\n", 
+                (unsigned)ntohs(sockaddr.sin_port),
+                (unsigned)portNumber),
+
+        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, 
+                _base_port + portNumber);
+        // fflush(stdout);
+    }
+
+    if (wait_for_connection) {
+        printf("Waiting for connection ....\n");
+        // fflush(stdout);
+        net_fd = accept(listen_fd, NULL, NULL);
+        if (net_fd == -1) {
+            printf("accept() error - %s", strerror(errno));
+            exit(1);
+        }
+        setsockopt(net_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
+        setsockopt(net_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
+        _connected = true;
+        _rd_fd = net_fd;
+        _wr_fd = net_fd;
+    }
+}
+
 /*
   shutdown a UART
  */
 void LinuxUARTDriver::end() 
 {
     _initialised = false;
+    _connected = false;
     while (_in_timer) hal.scheduler->delay(1);
     if (_rd_fd == _wr_fd && _rd_fd != -1) {
         close(_rd_fd);

libraries/AP_HAL_Linux/UARTDriver.h

@@ -25,18 +25,22 @@ public:
     size_t write(uint8_t c);
     size_t write(const uint8_t *buffer, size_t size);
 
-    void set_device_path(const char *path);
+    void set_device_path(char *path);
 
     void _timer_tick(void);
 
 private:
-    const char *device_path;
+    char *device_path;
     int _rd_fd;
     int _wr_fd;
     bool _nonblocking_writes;
     bool _console;
     volatile bool _initialised;
     volatile bool _in_timer;
+    uint16_t _base_port;
+    char *_ip;
+    char *_flag;
+    bool _connected; // true if a client has connected         
 
     // we use in-task ring buffers to reduce the system call cost
     // of ::read() and ::write() in the main loop
@@ -55,6 +59,8 @@ private:
 
     int _write_fd(const uint8_t *buf, uint16_t n);
     int _read_fd(uint8_t *buf, uint16_t n);
+    void _tcp_start_connection(bool wait_for_connection);
+    int _parseDevicePath(char* arg);
     uint64_t _last_write_time;    
 };