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

@@ -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;
-        }
-
-        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;
-        }
+        return 1;
     }
 
-    return 1;
+    iovec[0].len = n;
+
+    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
+    }
+
+    tail = (tail + len) % size;
+    return true;
 }
 
 uint32_t ByteBuffer::read(uint8_t *data, uint32_t len)

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;