diff --git a/Makefile b/Makefile index 102c9156..528c2d0b 100644 --- a/Makefile +++ b/Makefile @@ -49,6 +49,7 @@ OBJECTS := \ payload.o \ pmgr.o \ proxy.o \ + ringbuffer.o \ smp.o \ start.o \ startup.o \ diff --git a/src/ringbuffer.c b/src/ringbuffer.c new file mode 100644 index 00000000..36b89d9b --- /dev/null +++ b/src/ringbuffer.c @@ -0,0 +1,81 @@ +#include "ringbuffer.h" +#include "malloc.h" +#include "types.h" + +ringbuffer_t *ringbuffer_alloc(size_t len) +{ + ringbuffer_t *bfr = malloc(sizeof(*bfr)); + if (!bfr) + return NULL; + + bfr->buffer = malloc(len); + if (!bfr->buffer) { + free(bfr); + return NULL; + } + + bfr->read = 0; + bfr->write = 0; + bfr->len = len; + + return bfr; +} + +void ringbuffer_free(ringbuffer_t *bfr) +{ + if (bfr) + free(bfr->buffer); + free(bfr); +} + +size_t ringbuffer_read(u8 *target, size_t len, ringbuffer_t *bfr) +{ + size_t read; + + for (read = 0; read < len; ++read) { + if (bfr->read == bfr->write) + break; + + *target = bfr->buffer[bfr->read]; + target++; + + bfr->read++; + bfr->read %= bfr->len; + } + + return read; +} + +size_t ringbuffer_write(const u8 *src, size_t len, ringbuffer_t *bfr) +{ + size_t written; + + for (written = 0; written < len; ++written) { + if (((bfr->write + 1) % bfr->len) == bfr->read) + break; + + bfr->buffer[bfr->write] = *src; + src++; + + bfr->write++; + bfr->write %= bfr->len; + } + + return written; +} + +size_t ringbuffer_get_used(ringbuffer_t *bfr) +{ + size_t read = bfr->read; + size_t write = bfr->write; + + if (write < read) + write += bfr->len; + + return write - read; +} + +size_t ringbuffer_get_free(ringbuffer_t *bfr) +{ + return bfr->len - ringbuffer_get_used(bfr); +} diff --git a/src/ringbuffer.h b/src/ringbuffer.h new file mode 100644 index 00000000..553ae767 --- /dev/null +++ b/src/ringbuffer.h @@ -0,0 +1,22 @@ +#ifndef RINGBUFFER_H +#define RINGBUFFER_H + +#include "types.h" + +typedef struct { + u8 *buffer; + size_t len; + size_t read; + size_t write; +} ringbuffer_t; + +ringbuffer_t *ringbuffer_alloc(size_t len); +void ringbuffer_free(ringbuffer_t *bfr); + +size_t ringbuffer_read(u8 *target, size_t len, ringbuffer_t *bfr); +size_t ringbuffer_write(const u8 *src, size_t len, ringbuffer_t *bfr); + +size_t ringbuffer_get_used(ringbuffer_t *bfr); +size_t ringbuffer_get_free(ringbuffer_t *bfr); + +#endif