From 26382e63dfdae14a10bf85ac61ed8f0a2279ab48 Mon Sep 17 00:00:00 2001
From: Staroselskii Georgii <gosha371@gmail.com>
Date: Thu, 11 Jun 2015 21:30:36 +0300
Subject: [PATCH] AP_HAL_Linux: made UARTDriver use UARTDevice

---
 libraries/AP_HAL_Linux/UARTDriver.cpp | 66 ++++++---------------------
 libraries/AP_HAL_Linux/UARTDriver.h   |  3 ++
 2 files changed, 17 insertions(+), 52 deletions(-)

diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp
index 3bcc69fbcb..a02d70b13c 100644
--- a/libraries/AP_HAL_Linux/UARTDriver.cpp
+++ b/libraries/AP_HAL_Linux/UARTDriver.cpp
@@ -24,6 +24,8 @@
 #include <arpa/inet.h>
 #include "../AP_HAL/utility/RingBuffer.h"
 
+#include "UARTDevice.h"
+
 extern const AP_HAL::HAL& hal;
 
 using namespace Linux;
@@ -102,7 +104,7 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
         case DEVICE_SERIAL:            
         {
             if (!_serial_start_connection()) {
-                return; /* Whatever it might mean */
+                break; /* Whatever it might mean */
             }
             break;
         }
@@ -123,19 +125,7 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
     _initialised = false;
     while (_in_timer) hal.scheduler->delay(1);
 
-    if (b != 0 && _rd_fd == _wr_fd) {
-        // set the baud rate
-        struct termios t;
-        memset(&t, 0, sizeof(t));
-        tcgetattr(_rd_fd, &t);
-        cfsetspeed(&t, b);
-        // disable LF -> CR/LF
-        t.c_iflag &= ~(BRKINT | ICRNL | IMAXBEL | IXON | IXOFF);
-        t.c_oflag &= ~(OPOST | ONLCR);
-        t.c_lflag &= ~(ISIG | ICANON | IEXTEN | ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE);
-        t.c_cc[VMIN] = 0;
-        tcsetattr(_rd_fd, TCSANOW, &t);
-    }
+    _device->set_speed(b);
 
     _allocate_buffers(rxS, txS);
 }
@@ -239,20 +229,9 @@ LinuxUARTDriver::device_type LinuxUARTDriver::_parseDevicePath(const char *arg)
 
 bool LinuxUARTDriver::_serial_start_connection()
 {
-    _rd_fd = open(device_path, O_RDWR);
-    _wr_fd = _rd_fd;
-
-    if (_rd_fd == -1) {
-        ::fprintf(stdout, "Failed to open UART device %s - %s\n",
-                  device_path, strerror(errno));
-        return false;
-    }
-
-    // always run the file descriptor non-blocking, and deal with
-    // blocking IO in the higher level calls
-    fcntl(_rd_fd, F_SETFL, fcntl(_rd_fd, F_GETFL, 0) | O_NONBLOCK);
-
-    // TODO: add proper flow control support
+    _device = new UARTDevice(device_path);
+    _device->open();
+    _device->set_blocking(false);
     _flow_control = FLOW_CONTROL_DISABLE;
 
     return true;
@@ -413,7 +392,7 @@ void LinuxUARTDriver::end()
     _connected = false;
     while (_in_timer) hal.scheduler->delay(1);
     if (_rd_fd == _wr_fd && _rd_fd != -1) {
-        close(_rd_fd);
+        _device->close();
     }
     _rd_fd = -1;
     _wr_fd = -1;
@@ -580,15 +559,8 @@ size_t LinuxUARTDriver::write(const uint8_t *buffer, size_t size)
 int LinuxUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
 {
     int ret = 0;
-
-    struct pollfd fds;
-    fds.fd = _wr_fd;
-    fds.events = POLLOUT;
-    fds.revents = 0;
-
-    if (poll(&fds, 1, 0) == 1) {
-        ret = ::write(_wr_fd, buf, n);
-    }
+    
+    ret = _device->write(buf, n);
 
     if (ret > 0) {
         BUF_ADVANCEHEAD(_writebuf, ret);
@@ -604,22 +576,12 @@ int LinuxUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
 int LinuxUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
 {
     int ret;
-    ret = ::read(_rd_fd, buf, n);
+
+    ret = _device->read(buf, n);
+
     if (ret > 0) {
         BUF_ADVANCETAIL(_readbuf, ret);
-    } else {
-        switch (errno) {
-            case EAGAIN: 
-                /* Ignore EAGAIN that resulted from non-blocking read */
-                break;
-            case EPIPE:
-                /* Ignore EPIPE that resulted from peer shutdown */
-                break;
-            default:
-               ::fprintf(stdout, "read failed - %s\n", strerror(errno));
-               break;
-        }
-    }
+    } 
 
     return ret;
 }
diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h
index 3a4e7cd986..6b6c7b8e98 100644
--- a/libraries/AP_HAL_Linux/UARTDriver.h
+++ b/libraries/AP_HAL_Linux/UARTDriver.h
@@ -4,6 +4,8 @@
 
 #include <AP_HAL_Linux.h>
 
+#include "SerialDevice.h"
+
 class Linux::LinuxUARTDriver : public AP_HAL::UARTDriver {
 public:
     LinuxUARTDriver(bool default_console);
@@ -32,6 +34,7 @@ public:
     enum flow_control get_flow_control(void) { return _flow_control; }
 
 private:
+    SerialDevice *_device = nullptr;
     int _rd_fd;
     int _wr_fd;
     bool _nonblocking_writes;