ring buffer fixes

1 files changed, 31 insertions, 0 deletions

diff --git a/code/core/rbuf.c b/code/core/rbuf.c
new file mode 100644
index 0000000..54843fa
--- /dev/null
+++ b/code/core/rbuf.c
@@ -0,0 +1,31 @@
+#include "core.h"
+
+#include <string.h>
+
+int ecp_rbuf_init(ECPRBuffer *rbuf, ECPRBMessage *msg, unsigned int msg_size) {
+    rbuf->msg = msg;
+    rbuf->msg_size = msg_size;
+    
+    return ECP_OK;
+}
+
+int ecp_rbuf_msg_idx(ECPRBuffer *rbuf, ecp_seq_t seq) {
+    ecp_seq_t seq_offset = seq - rbuf->seq_start;
+
+    // This also checks for seq_start <= seq if seq type range >> rbuf->msg_size
+    if (seq_offset < rbuf->msg_size) return ECP_RBUF_IDX_MASK(rbuf->msg_start + seq_offset, rbuf->msg_size);
+    return ECP_ERR_RBUF_IDX;
+}
+
+ssize_t ecp_rbuf_msg_store(ECPRBuffer *rbuf, ecp_seq_t seq, unsigned char *msg, size_t msg_size, unsigned char test_flags, unsigned char set_flags) {
+    int idx = ecp_rbuf_msg_idx(rbuf, seq);
+    if (idx < 0) return idx;
+    if (test_flags && (test_flags & rbuf->msg[idx].flags)) return ECP_ERR_RBUF_FLAG;
+    
+    memcpy(rbuf->msg[idx].msg, msg, msg_size);
+    rbuf->msg[idx].size = msg_size;
+    rbuf->msg[idx].flags = set_flags;
+
+    return msg_size;
+}
+