From 546ed19ffcccb5a14273ee35f9df5ccdac153a59 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 22 Jan 2013 10:50:26 +1100
Subject: [PATCH] HAL_PX4: improved efficiency of serial writes

use buffer at a time writes when possible, and use O_NONBLOCKING to
avoid txspace()
---
 libraries/AP_HAL_PX4/UARTDriver.cpp | 47 +++++++++++++++++++++--------
 libraries/AP_HAL_PX4/UARTDriver.h   |  2 +-
 2 files changed, 35 insertions(+), 14 deletions(-)

diff --git a/libraries/AP_HAL_PX4/UARTDriver.cpp b/libraries/AP_HAL_PX4/UARTDriver.cpp
index 5127c9d0ce..8397d0cadf 100644
--- a/libraries/AP_HAL_PX4/UARTDriver.cpp
+++ b/libraries/AP_HAL_PX4/UARTDriver.cpp
@@ -1,3 +1,5 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
 #include <AP_HAL.h>
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
@@ -69,8 +71,16 @@ void PX4UARTDriver::begin(uint32_t b)
 void PX4UARTDriver::end() {}
 void PX4UARTDriver::flush() {}
 bool PX4UARTDriver::is_initialized() { return true; }
-void PX4UARTDriver::set_blocking_writes(bool blocking) {
-	_nonblocking_writes = !blocking;
+void PX4UARTDriver::set_blocking_writes(bool blocking) 
+{
+	unsigned v;
+    v = fcntl(_fd, F_GETFL, 0);
+    if (blocking) {
+        v &= ~O_NONBLOCK;
+    } else {
+        v |= O_NONBLOCK;
+    }
+    fcntl(_fd, F_SETFL, v);
 }
 bool PX4UARTDriver::tx_pending() { return false; }
 
@@ -144,14 +154,11 @@ int16_t PX4UARTDriver::read() {
 	}
 	if (_readbuf_count == 0) {
 		// refill the read buffer
-		int16_t avail = available();
-		if (avail > 0) {
-			int n = ::read(_fd, _readbuf, _readbuf_size);
-			if (n > 0) {
-				_readbuf_count = n;
-				_readbuf_ofs = 0;
-			}
-		}
+        int n = ::read(_fd, _readbuf, _readbuf_size);
+        if (n > 0) {
+            _readbuf_count = n;
+            _readbuf_ofs = 0;
+        }
 	}
 	if (_readbuf_count == 0) {
 		return -1;
@@ -163,14 +170,28 @@ int16_t PX4UARTDriver::read() {
 }
 
 /* PX4 implementations of Print virtual methods */
-size_t PX4UARTDriver::write(uint8_t c) { 
+size_t PX4UARTDriver::write(uint8_t c) 
+{ 
 	if (!_initialised) {
 		return 0;
 	}
-	if (_nonblocking_writes && txspace() == 0) {
+	int ret = ::write(_fd, &c, 1);
+    if (ret == -1) {
+        ret = 0;
+    }
+    return ret;
+}
+
+size_t PX4UARTDriver::write(const uint8_t *buffer, size_t size)
+{
+	if (!_initialised) {
 		return 0;
 	}
-	return ::write(_fd, &c, 1);
+	int ret = ::write(_fd, buffer, size);
+    if (ret == -1) {
+        ret = 0;
+    }
+    return ret;
 }
 
 // handle %S -> %s
diff --git a/libraries/AP_HAL_PX4/UARTDriver.h b/libraries/AP_HAL_PX4/UARTDriver.h
index 45e1e57325..6f7cef1591 100644
--- a/libraries/AP_HAL_PX4/UARTDriver.h
+++ b/libraries/AP_HAL_PX4/UARTDriver.h
@@ -32,6 +32,7 @@ public:
 
     /* PX4 implementations of Print virtual methods */
     size_t write(uint8_t c);
+    size_t write(const uint8_t *buffer, size_t size);
 
     bool _initialised;
 
@@ -42,7 +43,6 @@ public:
 private:
     const char *_devpath;
     int _fd;
-    bool _nonblocking_writes;
     void _vdprintf(int fd, const char *fmt, va_list ap);
 
     // we keep a small read buffer to lower the cost of ::read() system calls