From b7dd4dad64ef0303d0da1755702e5c00d7abe7cd Mon Sep 17 00:00:00 2001
From: Murilo Belluzzo <murilo.belluzzo@intel.com>
Date: Thu, 23 Jun 2016 18:47:24 -0300
Subject: [PATCH] AP_HAL: Fix ByteBuffer::reserve() breaking buffer state

When using reserved(), the reserved memory cannot be read before it's
written, therefore we cannot update 'tail' until the caller of
reserved() is done writing.

To solve that, a method called 'commit()' was added so the caller can
inform that is done with the memory usage and is safe to update 'tail'.
The caller also has to inform the length that was actually written.

This solution was developed to work considering the usage context of
this class: 1 reader and 1 writer **only**.
---
 libraries/AP_HAL/utility/RingBuffer.cpp | 46 +++++++++++++++----------
 libraries/AP_HAL/utility/RingBuffer.h   | 13 +++++--
 2 files changed, 39 insertions(+), 20 deletions(-)

diff --git a/libraries/AP_HAL/utility/RingBuffer.cpp b/libraries/AP_HAL/utility/RingBuffer.cpp
index c1350b1655..34df541703 100644
--- a/libraries/AP_HAL/utility/RingBuffer.cpp
+++ b/libraries/AP_HAL/utility/RingBuffer.cpp
@@ -62,6 +62,7 @@ uint32_t ByteBuffer::write(const uint8_t *data, uint32_t len)
         ret += vec[i].len;
     }
 
+    commit(ret);
     return ret;
 }
 
@@ -142,36 +143,45 @@ uint32_t ByteBuffer::peekbytes(uint8_t *data, uint32_t len)
     return ret;
 }
 
-int ByteBuffer::reserve(ByteBuffer::IoVec iovec[2], uint32_t len)
+uint8_t ByteBuffer::reserve(ByteBuffer::IoVec iovec[2], uint32_t len)
 {
-    if (len > space()) {
-        len = space();
+    uint32_t n = space();
+
+    if (len > n) {
+        len = n;
     }
+
     if (!len) {
         return 0;
     }
 
     iovec[0].data = &buf[tail];
-    if (tail+len <= size) {
+
+    n = size - tail;
+    if (len <= n) {
         iovec[0].len = len;
-    } else {
-        auto n = size - tail;
-        if (n > len) {
-            n = len;
-        }
+        return 1;
+    }
 
-        iovec[0].len = n;
+    iovec[0].len = n;
 
-        tail = (tail + n) % size;
-        n = len - n;
-        if (n > 0) {
-            iovec[1].data = &buf[tail];
-            iovec[1].len = n;
-            return 2;
-        }
+    iovec[1].data = buf;
+    iovec[1].len = len - n;
+
+    return 2;
+}
+
+/*
+ * Advance the writer pointer by 'len'
+ */
+bool ByteBuffer::commit(uint32_t len)
+{
+    if (len > space()) {
+        return false; //Someone broke the agreement
     }
 
-    return 1;
+    tail = (tail + len) % size;
+    return true;
 }
 
 uint32_t ByteBuffer::read(uint8_t *data, uint32_t len)
diff --git a/libraries/AP_HAL/utility/RingBuffer.h b/libraries/AP_HAL/utility/RingBuffer.h
index 9db1e561d0..e021eb7f8e 100644
--- a/libraries/AP_HAL/utility/RingBuffer.h
+++ b/libraries/AP_HAL/utility/RingBuffer.h
@@ -86,8 +86,17 @@ public:
     // ring buffer (if wraparound is happening), or just one contiguous
     // part. Returns the number of `vec` elements filled out. Can be used
     // with system calls such as `readv()`.
-    int reserve(IoVec vec[2], uint32_t len);
-    
+    //
+    // After a call to 'reserve()', 'write()' should never be called
+    // until 'commit()' is called!
+    uint8_t reserve(IoVec vec[2], uint32_t len);
+
+    /*
+     * "Releases" the memory previously reserved by 'reserve()' to be read.
+     * Committer must inform how many bytes were actually written in 'len'.
+     */
+    bool commit(uint32_t len);
+
 private:
     uint8_t *buf = nullptr;
     uint32_t size = 0;