From ea362ea8f9a2059ec250fc451ce35f0fb82c581a Mon Sep 17 00:00:00 2001
From: Sven Peter <sven@svenpeter.dev>
Date: Sat, 13 Mar 2021 20:16:21 +0100
Subject: [PATCH] ringbuffer: add a simple ringbuffer data structure

Signed-off-by: Sven Peter <sven@svenpeter.dev>
---
 Makefile         |  1 +
 src/ringbuffer.c | 81 ++++++++++++++++++++++++++++++++++++++++++++++++
 src/ringbuffer.h | 22 +++++++++++++
 3 files changed, 104 insertions(+)
 create mode 100644 src/ringbuffer.c
 create mode 100644 src/ringbuffer.h

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