AP_HAL: Add method to peek non-contiguous parts of a ByteBuffer

Modify ByteBuffer class to have a `peekiovec()` method, that takes in a
`struct IoVec` array (similar to `struct iovec` from POSIX), and a
number of bytes, and returns the number of elements from this array
that have been filled out.  It is either 0 (buffer is empty), 1
(there's enough contiguous bytes to read that amount) or 2 (ring buffer
is wrapping around).

This enables using scatter-gather I/O (i.e. writev()), removing calls
to memcpy().  That's one call when no wrap-around is happening, and
two calls if it is.

Also, rewrite `ByteBuffer::peekbytes()` to use `peekiovec()`, so that
some of the checks performed by the former are not replicated in the
latter.

libraries/AP_HAL/utility/RingBuffer.cpp

@@ -1,4 +1,5 @@
 #include "RingBuffer.h"
+#include <algorithm>
 #include <stdlib.h>
 #include <string.h>
 
@@ -109,10 +110,7 @@ bool ByteBuffer::advance(uint32_t n)
     return true;
 }
 
-/*
+int ByteBuffer::peekiovec(ByteBuffer::IoVec iovec[2], uint32_t len)
-  read len bytes without advancing the read pointer
- */
-uint32_t ByteBuffer::peekbytes(uint8_t *data, uint32_t len)
 {
     if (len > available()) {
         len = available();
@@ -121,22 +119,42 @@ uint32_t ByteBuffer::peekbytes(uint8_t *data, uint32_t len)
         return 0;
     }
     uint32_t n;
-    const uint8_t *b = readptr(n);
+    auto b = readptr(n);
     if (n > len) {
         n = len;
     }
 
-    // perform first memcpy
+    iovec[0].data = const_cast<uint8_t *>(b);
-    memcpy(data, b, n);
+    iovec[0].len = n;
-    data += n;
 
     if (len > n) {
-        // possible second memcpy, must be from front of buffer
+        iovec[1].data = buf;
-        memcpy(data, buf, len-n);
+        iovec[1].len = len - n;
+
+        return 2;
     }
-    return len;
+
+    return 1;
 }
 
+/*
+  read len bytes without advancing the read pointer
+ */
+uint32_t ByteBuffer::peekbytes(uint8_t *data, uint32_t len)
+{
+    ByteBuffer::IoVec vec[2];
+    const auto n_vec = peekiovec(vec, len);
+    uint32_t ret = 0;
+
+    for (int i = 0; i < n_vec; i++) {
+        memcpy(data + ret, vec[i].data, vec[i].len);
+        ret += vec[i].len;
+    }
+
+    return ret;
+}
+
+
 uint32_t ByteBuffer::read(uint8_t *data, uint32_t len)
 {
     uint32_t ret = peekbytes(data, len);

libraries/AP_HAL/utility/RingBuffer.h

@@ -72,6 +72,15 @@ public:
       read len bytes without advancing the read pointer
     */
     uint32_t peekbytes(uint8_t *data, uint32_t len);
+
+    // Similar to peekbytes(), but will fill out IoVec struct with
+    // both parts of the ring buffer if wraparound is happpening, or
+    // just one part. Returns the number of parts written to.
+    struct IoVec {
+        uint8_t *data;
+        uint32_t len;
+    };
+    int peekiovec(IoVec vec[2], uint32_t len);
     
 private:
     uint8_t *buf = nullptr;