ardupilot/libraries/AP_HAL/utility/RingBuffer.cpp

253 lines
4.3 KiB
C++

#include <stdlib.h>
#include <string.h>
#include "RingBuffer.h"
ByteBuffer::ByteBuffer(uint32_t _size)
{
buf = (uint8_t*)malloc(_size);
size = buf ? _size : 0;
}
ByteBuffer::~ByteBuffer(void)
{
free(buf);
}
/*
* Caller is responsible for locking in set_size()
*/
bool ByteBuffer::set_size(uint32_t _size)
{
head = tail = 0;
if (_size != size) {
free(buf);
buf = (uint8_t*)malloc(_size);
if (!buf) {
size = 0;
return false;
}
size = _size;
}
return true;
}
uint32_t ByteBuffer::available(void) const
{
/* use a copy on stack to avoid race conditions of @tail being updated by
* the writer thread */
uint32_t _tail = tail;
if (head > _tail) {
return size - head + _tail;
}
return _tail - head;
}
void ByteBuffer::clear(void)
{
head = tail = 0;
}
uint32_t ByteBuffer::space(void) const
{
if (size == 0) {
return 0;
}
/* use a copy on stack to avoid race conditions of @head being updated by
* the reader thread */
uint32_t _head = head;
uint32_t ret = 0;
if (_head <= tail) {
ret = size;
}
ret += _head - tail - 1;
return ret;
}
bool ByteBuffer::empty(void) const
{
return head == tail;
}
uint32_t ByteBuffer::write(const uint8_t *data, uint32_t len)
{
ByteBuffer::IoVec vec[2];
const auto n_vec = reserve(vec, len);
uint32_t ret = 0;
for (int i = 0; i < n_vec; i++) {
memcpy(vec[i].data, data + ret, vec[i].len);
ret += vec[i].len;
}
commit(ret);
return ret;
}
/*
update bytes at the read pointer. Used to update an object without
popping it
*/
bool ByteBuffer::update(const uint8_t *data, uint32_t len)
{
if (len > available()) {
return false;
}
// perform as two memcpy calls
uint32_t n = size - head;
if (n > len) {
n = len;
}
memcpy(&buf[head], data, n);
data += n;
if (len > n) {
memcpy(&buf[0], data, len-n);
}
return true;
}
bool ByteBuffer::advance(uint32_t n)
{
if (n > available()) {
return false;
}
head = (head + n) % size;
return true;
}
uint8_t ByteBuffer::peekiovec(ByteBuffer::IoVec iovec[2], uint32_t len)
{
uint32_t n = available();
if (len > n) {
len = n;
}
if (len == 0) {
return 0;
}
auto b = readptr(n);
if (n > len) {
n = len;
}
iovec[0].data = const_cast<uint8_t *>(b);
iovec[0].len = n;
if (len <= n) {
return 1;
}
iovec[1].data = buf;
iovec[1].len = len - n;
return 2;
}
/*
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;
}
uint8_t ByteBuffer::reserve(ByteBuffer::IoVec iovec[2], uint32_t len)
{
uint32_t n = space();
if (len > n) {
len = n;
}
if (!len) {
return 0;
}
iovec[0].data = &buf[tail];
n = size - tail;
if (len <= n) {
iovec[0].len = len;
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)
{
uint32_t ret = peekbytes(data, len);
advance(ret);
return ret;
}
bool ByteBuffer::read_byte(uint8_t *data)
{
if (!data) {
return false;
}
int16_t ret = peek(0);
if (ret < 0) {
return false;
}
*data = ret;
return advance(1);
}
/*
* Returns the pointer and size to a contiguous read in the buffer
*/
const uint8_t *ByteBuffer::readptr(uint32_t &available_bytes)
{
uint32_t _tail = tail;
available_bytes = (head > _tail) ? size - head : _tail - head;
return available_bytes ? &buf[head] : nullptr;
}
int16_t ByteBuffer::peek(uint32_t ofs) const
{
if (ofs >= available()) {
return -1;
}
return buf[(head+ofs)%size];
}