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;