From c373429a6e16d4dda687a9090117fce5f3a8bc7e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 20 Feb 2013 18:15:56 +1100
Subject: [PATCH] HAL_PX4: fixed a buffer handling bug

BUF_SPACE() was badly buggy, which could lead to memory corruption
---
 libraries/AP_HAL_PX4/UARTDriver.cpp | 25 ++++++++++++++++---------
 1 file changed, 16 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_HAL_PX4/UARTDriver.cpp b/libraries/AP_HAL_PX4/UARTDriver.cpp
index 15b8c9b031..e119ba1108 100644
--- a/libraries/AP_HAL_PX4/UARTDriver.cpp
+++ b/libraries/AP_HAL_PX4/UARTDriver.cpp
@@ -15,6 +15,7 @@
 #include <fcntl.h>
 #include <termios.h>
 #include <drivers/drv_hrt.h>
+#include <assert.h>
 
 using namespace PX4;
 
@@ -47,7 +48,6 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
         v = fcntl(_fd, F_GETFL, 0);
         fcntl(_fd, F_SETFL, v | O_NONBLOCK);
 
-		_initialised = true;
         if (rxS == 0) {
             rxS = 128;
         }
@@ -56,6 +56,9 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
         }
 	}
 
+    _initialised = false;
+    while (_in_timer) hal.scheduler->delay(1);
+
 	if (b != 0) {
 		// set the baud rate
 		struct termios t;
@@ -70,8 +73,6 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
       allocate the read buffer
      */
 	if (rxS != 0 && rxS != _readbuf_size) {
-        _initialised = false;
-        while (_in_timer) hal.scheduler->delay(1);
 		_readbuf_size = rxS;
 		if (_readbuf != NULL) {
 			free(_readbuf);
@@ -79,24 +80,24 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
 		_readbuf = (uint8_t *)malloc(_readbuf_size);
 		_readbuf_head = 0;
 		_readbuf_tail = 0;
-        _initialised = true;
 	}
 
     /*
       allocate the write buffer
      */
 	if (txS != 0 && txS != _writebuf_size) {
-        _initialised = false;
-        while (_in_timer) hal.scheduler->delay(1);
 		_writebuf_size = txS;
 		if (_writebuf != NULL) {
 			free(_writebuf);
 		}
-		_writebuf = (uint8_t *)malloc(_writebuf_size);
+		_writebuf = (uint8_t *)malloc(_writebuf_size+16);
 		_writebuf_head = 0;
 		_writebuf_tail = 0;
-        _initialised = true;
 	}
+
+    if (_writebuf_size != 0 && _readbuf_size != 0) {
+        _initialised = true;
+    }
 }
 
 void PX4UARTDriver::begin(uint32_t b) 
@@ -191,7 +192,7 @@ void PX4UARTDriver::_vprintf(const char *fmt, va_list ap)
   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)?(buf##_tail - _head) - 1:((buf##_size - buf##_tail) + _head) - 1)
+#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
@@ -284,6 +285,7 @@ size_t PX4UARTDriver::write(const uint8_t *buffer, size_t size)
     }
     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;
@@ -292,11 +294,13 @@ size_t PX4UARTDriver::write(const uint8_t *buffer, size_t 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);
     }        
@@ -380,17 +384,20 @@ void PX4UARTDriver::_timer_tick(void)
         perf_begin(_perf_uart);
         if (_readbuf_tail < _head) {
             // one read will do
+            assert(_readbuf_tail+n <= _readbuf_size);
             int ret = ::read(_fd, &_readbuf[_readbuf_tail], n);
             if (ret > 0) {
                 BUF_ADVANCETAIL(_readbuf, ret);
             }
         } else {
             uint16_t n1 = _readbuf_size - _readbuf_tail;
+            assert(_readbuf_tail+n1 <= _readbuf_size);
             int ret = ::read(_fd, &_readbuf[_readbuf_tail], n1);
             if (ret > 0) {
                 BUF_ADVANCETAIL(_readbuf, ret);
             }
             if (ret == n1 && n != n1) {
+                assert(_readbuf_tail+(n-n1) <= _readbuf_size);
                 ret = ::read(_fd, &_readbuf[_readbuf_tail], n - n1);                
                 if (ret > 0) {
                     BUF_ADVANCETAIL(_readbuf, ret);