HAL_Linux: added buffering on the UARTs

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -4,6 +4,7 @@
 
 #include "Scheduler.h"
 #include "Storage.h"
+#include "UARTDriver.h"
 #include <unistd.h>
 #include <sys/time.h>
 #include <poll.h>
@@ -203,7 +204,10 @@ void *LinuxScheduler::_uart_thread(void)
     while (true) {
         poll(NULL, 0, 1);
 
-        // process any pending serial bytes: TODO
+        // process any pending serial bytes
+        ((LinuxUARTDriver *)hal.uartA)->_timer_tick();
+        ((LinuxUARTDriver *)hal.uartB)->_timer_tick();
+        ((LinuxUARTDriver *)hal.uartC)->_timer_tick();
     }
     return NULL;
 }

libraries/AP_HAL_Linux/UARTDriver.cpp

@@ -12,8 +12,12 @@
 #include <sys/stat.h>
 #include <fcntl.h>
 #include <unistd.h>
+#include <poll.h>
+#include <assert.h>
 #include <sys/ioctl.h>
 
+extern const AP_HAL::HAL& hal;
+
 using namespace Linux;
 
 LinuxUARTDriver::LinuxUARTDriver() :
@@ -35,36 +39,96 @@ void LinuxUARTDriver::set_device_path(const char *path)
  */
 void LinuxUARTDriver::begin(uint32_t b) 
 {
-    if (device_path == NULL) {
-        return;
-    }
+    begin(b, 0, 0);
+}
 
-    if (_fd == -1) {
-        _fd = open(device_path, O_RDWR);
+void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) 
+{
+    if (!_initialised) {
+        if (device_path == NULL) {
+            return;
+        }
+        uint8_t retries = 0;
+        while (retries < 5) {
+            _fd = open(device_path, O_RDWR);
+            if (_fd != -1) {
+                break;
+            }
+            // sleep a bit and retry. There seems to be a NuttX bug
+            // that can cause ttyACM0 to not be available immediately,
+            // but a small delay can fix it
+            hal.scheduler->delay(100);
+            retries++;
+        }
         if (_fd == -1) {
-            ::printf("UARTDriver: Failed to open %s - %s\n", 
-                     device_path,
-                     strerror(errno));
+            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
+        // blocking IO in the higher level calls
+        fcntl(_fd, F_SETFL, fcntl(_fd, F_GETFL, 0) | O_NONBLOCK);
+        
+        if (rxS == 0) {
+            rxS = 128;
+        }
+
+        // we have enough memory to have a larger transmit buffer for
+        // all ports. This means we don't get delays while waiting to
+        // write GPS config packets
+        if (txS < 512) {
+            txS = 512;
+        }
     }
 
-    /* if baudrate has been specified, then set the baudrate */
+    _initialised = false;
+    while (_in_timer) hal.scheduler->delay(1);
+
     if (b != 0) {
+        // set the baud rate
         struct termios t;
         tcgetattr(_fd, &t);
         cfsetspeed(&t, b);
-
         // disable LF -> CR/LF
         t.c_oflag &= ~ONLCR;
         tcsetattr(_fd, TCSANOW, &t);
     }
-}
 
-void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) 
-{
-    // ignore buffer sizes for now
-    begin(b);
+    /*
+      allocate the read buffer
+    */
+    if (rxS != 0 && rxS != _readbuf_size) {
+        _readbuf_size = rxS;
+        if (_readbuf != NULL) {
+            free(_readbuf);
+        }
+        _readbuf = (uint8_t *)malloc(_readbuf_size);
+        _readbuf_head = 0;
+        _readbuf_tail = 0;
+    }
+
+    /*
+      allocate the write buffer
+    */
+    if (txS != 0 && txS != _writebuf_size) {
+        _writebuf_size = txS;
+        if (_writebuf != NULL) {
+            free(_writebuf);
+        }
+        _writebuf = (uint8_t *)malloc(_writebuf_size+16);
+        _writebuf_head = 0;
+        _writebuf_tail = 0;
+    }
+
+    if (_writebuf_size != 0 && _readbuf_size != 0) {
+        _initialised = true;
+    }
 }
 
 /*
@@ -72,10 +136,25 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
  */
 void LinuxUARTDriver::end() 
 {
+    _initialised = false;
+    while (_in_timer) hal.scheduler->delay(1);
     if (_fd != -1) {
         close(_fd);
         _fd = -1;
     }
+    if (_readbuf) {
+        free(_readbuf);
+        _readbuf = NULL;
+    }
+    if (_writebuf) {
+        free(_writebuf);
+        _writebuf = NULL;
+    }
+    _readbuf_size = _writebuf_size = 0;
+    _writebuf_head = 0;
+    _writebuf_tail = 0;
+    _readbuf_head = 0;
+    _readbuf_tail = 0;
 }
 
 
@@ -90,7 +169,7 @@ void LinuxUARTDriver::flush()
  */
 bool LinuxUARTDriver::is_initialized() 
 {
-    return (_fd != -1);
+    return _initialised;
 }
 
 
@@ -99,48 +178,37 @@ bool LinuxUARTDriver::is_initialized()
  */
 void LinuxUARTDriver::set_blocking_writes(bool blocking) 
 {
-    unsigned v;
-    if (_fd == -1) {
-        return;
-    }
+    _nonblocking_writes = !blocking;
+}
 
-    v = fcntl(_fd, F_GETFL, 0);
-    
-    if (blocking) {
-        v &= ~O_NONBLOCK;
-    } else {
-        v |= O_NONBLOCK;
-    }
 
-    fcntl(_fd, F_SETFL, v);    
-}
+/*
+  buffer handling macros
+ */
+#define BUF_AVAILABLE(buf) ((buf##_head > (_tail=buf##_tail))? (buf##_size - buf##_head) + _tail: _tail - buf##_head)
+#define BUF_SPACE(buf) (((_head=buf##_head) > buf##_tail)?(_head - buf##_tail) - 1:((buf##_size - buf##_tail) + _head) - 1)
+#define BUF_EMPTY(buf) (buf##_head == buf##_tail)
+#define BUF_ADVANCETAIL(buf, n) buf##_tail = (buf##_tail + n) % buf##_size
+#define BUF_ADVANCEHEAD(buf, n) buf##_head = (buf##_head + n) % buf##_size
 
 /*
   do we have any bytes pending transmission?
  */
 bool LinuxUARTDriver::tx_pending() 
 { 
-    // no buffering, so always false
-    return false; 
+    return !BUF_EMPTY(_writebuf);
 }
 
-
 /*
   return the number of bytes available to be read
  */
 int16_t LinuxUARTDriver::available() 
 { 
-    int nread;
-    if (_fd == -1) {
+    if (!_initialised) {
         return 0;
     }
-
-    nread = 0;
-    if (ioctl(_fd, FIONREAD, (unsigned long)&nread) == 0) {
-        return nread;
-    }
-    // ioctl failed??
-    return 0;
+    uint16_t _tail;
+    return BUF_AVAILABLE(_readbuf);
 }
 
 /*
@@ -148,33 +216,195 @@ int16_t LinuxUARTDriver::available()
  */
 int16_t LinuxUARTDriver::txspace() 
 { 
-    // for now lie and say we always have 128, we will need a ring
-    // buffer later and a IO thread
-    return 128; 
+    if (!_initialised) {
+        return 0;
+    }
+    uint16_t _head;
+    return BUF_SPACE(_writebuf);
 }
 
 int16_t LinuxUARTDriver::read() 
 { 
-    char c;
-    if (_fd == -1) {
+    uint8_t c;
+    if (!_initialised || _readbuf == NULL) {
         return -1;
     }
-    if (::read(_fd, &c, 1) == 1) {
-        return (int16_t)c;
+    if (BUF_EMPTY(_readbuf)) {
+        return -1;
     }
-    return -1;
+    c = _readbuf[_readbuf_head];
+    BUF_ADVANCEHEAD(_readbuf, 1);
+    return c;
 }
 
 /* Linux implementations of Print virtual methods */
 size_t LinuxUARTDriver::write(uint8_t c) 
 { 
-    if (_fd == -1) {
+    if (!_initialised) {
+        return 0;
+    }
+    if (hal.scheduler->in_timerprocess()) {
+        // not allowed from timers
+        return 0;
+    }
+    uint16_t _head;
+
+    while (BUF_SPACE(_writebuf) == 0) {
+        if (_nonblocking_writes) {
+            return 0;
+        }
+        hal.scheduler->delay(1);
+    }
+    _writebuf[_writebuf_tail] = c;
+    BUF_ADVANCETAIL(_writebuf, 1);
+    return 1;
+}
+
+/*
+  write size bytes to the write buffer
+ */
+size_t LinuxUARTDriver::write(const uint8_t *buffer, size_t size)
+{
+    if (!_initialised) {
+        return 0;
+    }
+    if (hal.scheduler->in_timerprocess()) {
+        // not allowed from timers
+        return 0;
+    }
+
+    if (!_nonblocking_writes) {
+        /*
+          use the per-byte delay loop in write() above for blocking writes
+         */
+        size_t ret = 0;
+        while (size--) {
+            if (write(*buffer++) != 1) break;
+            ret++;
+        }
+        return ret;
+    }
+
+    uint16_t _head, space;
+    space = BUF_SPACE(_writebuf);
+    if (space == 0) {
         return 0;
     }
-    if (::write(_fd, &c, 1) == 1) {
-        return 1;
+    if (size > space) {
+        size = space;
+    }
+    if (_writebuf_tail < _head) {
+        // perform as single memcpy
+        assert(_writebuf_tail+size <= _writebuf_size);
+        memcpy(&_writebuf[_writebuf_tail], buffer, size);
+        BUF_ADVANCETAIL(_writebuf, size);
+        return size;
+    }
+
+    // perform as two memcpy calls
+    uint16_t n = _writebuf_size - _writebuf_tail;
+    if (n > size) n = size;
+    assert(_writebuf_tail+n <= _writebuf_size);
+    memcpy(&_writebuf[_writebuf_tail], buffer, n);
+    BUF_ADVANCETAIL(_writebuf, n);
+    buffer += n;
+    n = size - n;
+    if (n > 0) {
+        assert(_writebuf_tail+n <= _writebuf_size);
+        memcpy(&_writebuf[_writebuf_tail], buffer, n);
+        BUF_ADVANCETAIL(_writebuf, n);
+    }        
+    return size;
+}
+
+/*
+  try writing n bytes, handling an unresponsive port
+ */
+int LinuxUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
+{
+    int ret = 0;
+
+    struct pollfd fds;
+    fds.fd = _fd;
+    fds.events = POLLOUT;
+    fds.revents = 0;
+
+    if (poll(&fds, 1, 0) == 1) {
+        ret = ::write(_fd, buf, n);
     }
-    return 0;
+
+    if (ret > 0) {
+        BUF_ADVANCEHEAD(_writebuf, ret);
+        return ret;
+    }
+
+    return ret;
+}
+
+/*
+  try reading n bytes, handling an unresponsive port
+ */
+int LinuxUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
+{
+    int ret;
+    ret = ::read(_fd, buf, n);
+    if (ret > 0) {
+        BUF_ADVANCETAIL(_readbuf, ret);
+    }
+    return ret;
+}
+
+
+/*
+  push any pending bytes to/from the serial port. This is called at
+  1kHz in the timer thread. Doing it this way reduces the system call
+  overhead in the main task enormously. 
+ */
+void LinuxUARTDriver::_timer_tick(void)
+{
+    uint16_t n;
+
+    if (!_initialised) return;
+
+    _in_timer = true;
+
+    // write any pending bytes
+    uint16_t _tail;
+    n = BUF_AVAILABLE(_writebuf);
+    if (n > 0) {
+        if (_tail > _writebuf_head) {
+            // do as a single write
+            _write_fd(&_writebuf[_writebuf_head], n);
+        } else {
+            // split into two writes
+            uint16_t n1 = _writebuf_size - _writebuf_head;
+            int ret = _write_fd(&_writebuf[_writebuf_head], n1);
+            if (ret == n1 && n != n1) {
+                _write_fd(&_writebuf[_writebuf_head], n - n1);                
+            }
+        }
+    }
+
+    // try to fill the read buffer
+    uint16_t _head;
+    n = BUF_SPACE(_readbuf);
+    if (n > 0) {
+        if (_readbuf_tail < _head) {
+            // one read will do
+            assert(_readbuf_tail+n <= _readbuf_size);
+            _read_fd(&_readbuf[_readbuf_tail], n);
+        } else {
+            uint16_t n1 = _readbuf_size - _readbuf_tail;
+            assert(_readbuf_tail+n1 <= _readbuf_size);
+            int ret = _read_fd(&_readbuf[_readbuf_tail], n1);
+            if (ret == n1 && n != n1) {
+                assert(_readbuf_tail+(n-n1) <= _readbuf_size);
+                _read_fd(&_readbuf[_readbuf_tail], n - n1);                
+            }
+        }
+    }
+
+    _in_timer = false;
 }
 
 #endif // CONFIG_HAL_BOARD

libraries/AP_HAL_Linux/UARTDriver.h

@@ -23,12 +23,37 @@ public:
 
     /* Linux implementations of Print virtual methods */
     size_t write(uint8_t c);
+    size_t write(const uint8_t *buffer, size_t size);
 
     void set_device_path(const char *path);
 
+    void _timer_tick(void);
+
 private:
     const char *device_path;
     int _fd;
+    bool _nonblocking_writes;
+    volatile bool _initialised;
+    volatile bool _in_timer;
+
+    // we use in-task ring buffers to reduce the system call cost
+    // of ::read() and ::write() in the main loop
+    uint8_t *_readbuf;
+    uint16_t _readbuf_size;
+
+    // _head is where the next available data is. _tail is where new
+    // data is put
+    volatile uint16_t _readbuf_head;
+    volatile uint16_t _readbuf_tail;
+
+    uint8_t *_writebuf;
+    uint16_t _writebuf_size;
+    volatile uint16_t _writebuf_head;
+    volatile uint16_t _writebuf_tail;
+
+    int _write_fd(const uint8_t *buf, uint16_t n);
+    int _read_fd(uint8_t *buf, uint16_t n);
+    uint64_t _last_write_time;
 };
 
 #endif // __AP_HAL_LINUX_UARTDRIVER_H__