114 lines
2.4 KiB
C
114 lines
2.4 KiB
C
#include "msg_buf.h"
|
|
|
|
typedef uint16_t len_prefix_t;
|
|
#define _PADDING_BYTE (0xFF)
|
|
#define _PADDING_VALUE ((len_prefix_t)0xFFFF)
|
|
#define _LENGTH_SIZE (sizeof(len_prefix_t))
|
|
|
|
|
|
|
|
int msg_buf_init(msg_buf_t *buf, void *storage, uint32_t capacity)
|
|
{
|
|
if ((capacity % _LENGTH_SIZE) != 0)
|
|
return ERR_ARGS;
|
|
|
|
buf->storage = storage;
|
|
buf->capacity = capacity;
|
|
buf->head = buf->count = 0;
|
|
|
|
return ERR_OK;
|
|
}
|
|
|
|
static inline void pad_to_end(msg_buf_t *buf)
|
|
{
|
|
//Pad to end, advancing head and count
|
|
uint32_t len = buf->capacity - buf->head;
|
|
memset(buf->storage + buf->head, _PADDING_BYTE, len);
|
|
buf->count += len;
|
|
buf->head = 0;
|
|
}
|
|
|
|
int msg_buf_push(msg_buf_t *buf, void *data, uint32_t len)
|
|
{
|
|
if (len > MAX_LEN)
|
|
return ERR_ARGS;
|
|
|
|
//Length of prefix+message
|
|
uint32_t len_f = _LENGTH_SIZE + len;
|
|
|
|
if (buf->capacity - buf->head < len_f) //No space ahead
|
|
{
|
|
if (buf->head - buf->count < len_f) //No space behind
|
|
return ERR_FULL;
|
|
|
|
pad_to_end(buf);
|
|
|
|
//Reset head
|
|
buf->head = 0;
|
|
}
|
|
|
|
//At this point head is at the first free location and there is enough space after it
|
|
*(len_prefix_t*)(buf->storage + buf->head) = len;
|
|
memcpy(buf->storage + buf->head + _LENGTH_SIZE, buf, len);
|
|
buf->head += len_f;
|
|
buf->count += len_f;
|
|
|
|
return ERR_OK;
|
|
}
|
|
|
|
static inline uint32_t buf_tail(msg_buf_t *buf)
|
|
{
|
|
if (buf->head >= buf->count)
|
|
return buf->head - buf->count;
|
|
|
|
return buf->capacity + buf->head - buf->count;
|
|
}
|
|
|
|
int msg_buf_peek(msg_buf_t *buf, void **ptr, uint32_t *len)
|
|
{
|
|
if (buf->count == 0)
|
|
return ERR_EMPTY;
|
|
|
|
uint32_t tail = buf_tail(buf);
|
|
//No need to worry about padding, pop will do cleanups
|
|
*len = *(uint16_t*)(buf->storage + tail);
|
|
*ptr = buf->storage + tail + _LENGTH_SIZE;
|
|
|
|
return ERR_OK;
|
|
}
|
|
|
|
int msg_buf_pop(msg_buf_t *buf)
|
|
{
|
|
if (buf->count == 0)
|
|
return ERR_EMPTY;
|
|
|
|
uint32_t tail = buf_tail(buf);
|
|
uint32_t len = *(uint16_t*)(buf->storage + tail);
|
|
buf->count -= _LENGTH_SIZE + len;
|
|
|
|
//If empty, reset head to start (minimize fragmentation)
|
|
if (buf->count == 0)
|
|
{
|
|
buf->head = 0;
|
|
return ERR_OK;
|
|
}
|
|
|
|
//At this point tail can either be at thestart of padding
|
|
// -> Clear padding (wraps back to 0)
|
|
|
|
tail = buf_tail(buf);
|
|
len = *(uint16_t*)(buf->storage + tail);
|
|
|
|
//Leave as is if not padding
|
|
if (len != _PADDING_VALUE)
|
|
return ERR_OK;
|
|
|
|
//Clear padding
|
|
uint32_t padding_len = buf->capacity - tail;
|
|
buf->count -= padding_len;
|
|
|
|
//Tail is now at storage start.
|
|
//If the buffer is empty, it is also aligned as a consequence.
|
|
return ERR_OK;
|
|
}
|