You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
197 lines
3.6 KiB
197 lines
3.6 KiB
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
#include <string.h>
|
|
|
|
#include "circbuf.h"
|
|
|
|
// --- Circbuf data structure ----
|
|
|
|
/** Offset in void* buffer */
|
|
#define PV_OFFS(pvBuf, elem_size, index) ((uint8_t*)(pvBuf) + ((elem_size)*(index)))
|
|
|
|
|
|
/**
|
|
* @brief Write data to a CircBuf slot
|
|
* @param cb : circbuf
|
|
* @param index : slot index
|
|
* @param source : data source
|
|
*/
|
|
static void write_buffer(CircBuf *cb, circbuf_size_t index, const void *source)
|
|
{
|
|
memcpy(PV_OFFS(cb->buf, cb->elem_size, index), source, cb->elem_size);
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Copy data from a CircBuf slot to a buffer
|
|
* @param cb : circbuf
|
|
* @param index : slot index
|
|
* @param dest : destination buffer
|
|
*/
|
|
static void read_buffer(const CircBuf *cb, circbuf_size_t index, void *dest)
|
|
{
|
|
memcpy(dest, PV_OFFS(cb->buf, cb->elem_size, index), cb->elem_size);
|
|
}
|
|
|
|
|
|
void cbuf_init(CircBuf *cb, void *buf, circbuf_size_t capacity, circbuf_size_t elem_size)
|
|
{
|
|
cb->buf = buf;
|
|
|
|
// set capacity, clear state
|
|
cb->elem_size = elem_size;
|
|
cb->cap = capacity;
|
|
|
|
cbuf_clear(cb);
|
|
}
|
|
|
|
|
|
/** Check if cbuf is full */
|
|
bool cbuf_full(const CircBuf *cb)
|
|
{
|
|
if (cb == NULL) {
|
|
return false;
|
|
}
|
|
|
|
return (cb->lr == cb->nw);
|
|
}
|
|
|
|
|
|
/** Check if cbuf is empty */
|
|
bool cbuf_empty(const CircBuf *cb)
|
|
{
|
|
if (cb == NULL) {
|
|
return true;
|
|
}
|
|
|
|
return ((cb->lr + 1) % cb->cap) == cb->nw;
|
|
}
|
|
|
|
|
|
bool cbuf_push(CircBuf *cb, const void *source)
|
|
{
|
|
if (cb == NULL || source == NULL || cbuf_full(cb)) {
|
|
return false;
|
|
}
|
|
|
|
write_buffer(cb, cb->nw, source);
|
|
|
|
// increment
|
|
cb->nw++;
|
|
if (cb->nw == cb->cap) { cb->nw = 0; }
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool cbuf_pop(CircBuf *cb, void *dest)
|
|
{
|
|
if (cb == NULL || dest == NULL || cbuf_empty(cb)) {
|
|
return false;
|
|
}
|
|
|
|
// increment
|
|
if (cb->nw == 0) {
|
|
cb->nw = cb->cap - 1;
|
|
} else {
|
|
cb->nw--;
|
|
}
|
|
|
|
read_buffer(cb, cb->nw, dest);
|
|
|
|
#ifdef CIRCBUF_ZERO_FREE_SLOTS
|
|
memset(PV_OFFS(cb->buf, cb->elem_size, cb->nw), 0, cb->elem_size);
|
|
#endif
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool cbuf_peek(const CircBuf *cb, void *dest) {
|
|
if (cb == NULL || dest == NULL || cbuf_empty(cb)) {
|
|
return false;
|
|
}
|
|
|
|
circbuf_size_t index = cb->nw;
|
|
if (index == 0) {
|
|
index = cb->cap - 1;
|
|
} else {
|
|
index--;
|
|
}
|
|
|
|
read_buffer(cb, index, dest);
|
|
return true;
|
|
}
|
|
|
|
|
|
bool cbuf_push_back(CircBuf *cb, const void *source)
|
|
{
|
|
if (cb == NULL || source == NULL || cbuf_full(cb)) {
|
|
return false;
|
|
}
|
|
|
|
write_buffer(cb, cb->lr, source);
|
|
|
|
// move lr back
|
|
if (cb->lr == 0) {
|
|
cb->lr = cb->cap - 1; // wrap to the end
|
|
} else {
|
|
cb->lr--;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool cbuf_peek_back(const CircBuf *cb, void *dest) {
|
|
if (cb == NULL || dest == NULL || cbuf_empty(cb)) {
|
|
return false;
|
|
}
|
|
|
|
circbuf_size_t index = cb->lr;
|
|
if (index == cb->cap - 1) {
|
|
index = 0;
|
|
} else {
|
|
index++;
|
|
}
|
|
|
|
read_buffer(cb, index, dest);
|
|
return true;
|
|
}
|
|
|
|
|
|
bool cbuf_pop_back(CircBuf *cb, void *dest)
|
|
{
|
|
if (cb == NULL || dest == NULL || cbuf_empty(cb)) {
|
|
return false;
|
|
}
|
|
|
|
// increment
|
|
cb->lr++;
|
|
if (cb->lr == cb->cap) {
|
|
cb->lr = 0;
|
|
}
|
|
|
|
read_buffer(cb, cb->lr, dest);
|
|
|
|
#ifdef CIRCBUF_ZERO_FREE_SLOTS
|
|
memset(PV_OFFS(cb->buf, cb->elem_size, cb->lr), 0, cb->elem_size);
|
|
#endif
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
void cbuf_clear(CircBuf *cb)
|
|
{
|
|
if (cb == NULL) {
|
|
return;
|
|
}
|
|
|
|
cb->lr = cb->cap - 1;
|
|
cb->nw = 0;
|
|
|
|
#ifdef CIRCBUF_ZERO_FREE_SLOTS
|
|
memset(PV_OFFS(cb->buf, cb->elem_size, 0), 0, cb->cap * cb->elem_size);
|
|
#endif
|
|
}
|
|
|