From fbefe320175259767719b6bc5a252ede558869f3 Mon Sep 17 00:00:00 2001
From: Leandro Pereira <leandro.pereira@intel.com>
Date: Thu, 19 May 2016 19:19:58 -0300
Subject: [PATCH] AP_HAL: Add method to reserve space in the ring buffer

Adds a method called `reserve()`, that will take a ByteBuffer::IoVec
array of at least two elements, and return the number of elements
filled out.  0 will be returned if `len` is over the total space of
the buffer; 1 will be returned if there's enough contiguous bytes in
the buffer; 2 will be returned if there are two non-contiguous blocks
of memory.

This method is suitable to be used with POSIX system calls such as
readv(), and is an optimization to not require temporary memory copies
while reading from a file descriptor.

Also modify the write() method to use reserve(), so that similar checks
are performed only in one place.
---
 libraries/AP_HAL/utility/RingBuffer.cpp | 65 +++++++++++++++----------
 libraries/AP_HAL/utility/RingBuffer.h   |  6 +++
 2 files changed, 45 insertions(+), 26 deletions(-)

diff --git a/libraries/AP_HAL/utility/RingBuffer.cpp b/libraries/AP_HAL/utility/RingBuffer.cpp
index a5e83ec896..620712323f 100644
--- a/libraries/AP_HAL/utility/RingBuffer.cpp
+++ b/libraries/AP_HAL/utility/RingBuffer.cpp
@@ -1,5 +1,4 @@
 #include "RingBuffer.h"
-#include <algorithm>
 #include <stdlib.h>
 #include <string.h>
 
@@ -50,33 +49,16 @@ bool ByteBuffer::empty(void) const
 
 uint32_t ByteBuffer::write(const uint8_t *data, uint32_t len)
 {
-    if (len > space()) {
-        len = space();
-    }
-    if (len == 0) {
-        return 0;
-    }
-    if (tail+len <= size) {
-        // perform as single memcpy
-        memcpy(&buf[tail], data, len);
-        tail = (tail + len) % size;
-        return len;
-    }
+    ByteBuffer::IoVec vec[2];
+    const auto n_vec = reserve(vec, len);
+    uint32_t ret = 0;
 
-    // perform as two memcpy calls
-    uint32_t n = size - tail;
-    if (n > len) {
-        n = len;
-    }
-    memcpy(&buf[tail], data, n);
-    tail = (tail + n) % size;
-    data += n;
-    n = len - n;
-    if (n > 0) {
-        memcpy(&buf[tail], data, n);
-        tail = (tail + n) % size;
+    for (int i = 0; i < n_vec; i++) {
+        memcpy(vec[i].data, data + ret, vec[i].len);
+        ret += vec[i].len;
     }
-    return len;
+
+    return ret;
 }
 
 /*
@@ -154,6 +136,37 @@ uint32_t ByteBuffer::peekbytes(uint8_t *data, uint32_t len)
     return ret;
 }
 
+int ByteBuffer::reserve(ByteBuffer::IoVec iovec[2], uint32_t len)
+{
+    if (len > space()) {
+        len = space();
+    }
+    if (!len) {
+        return 0;
+    }
+
+    iovec[0].data = &buf[tail];
+    if (tail+len <= size) {
+        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;
+}
 
 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 40bb2f5b72..eb63eff081 100644
--- a/libraries/AP_HAL/utility/RingBuffer.h
+++ b/libraries/AP_HAL/utility/RingBuffer.h
@@ -81,6 +81,12 @@ public:
         uint32_t len;
     };
     int peekiovec(IoVec vec[2], uint32_t len);
+
+    // Reserve `len` bytes and fills out `vec` with both parts of the
+    // 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);
     
 private:
     uint8_t *buf = nullptr;