1 files changed, 86 insertions, 15 deletions

diff --git a/kern/cbuf.c b/kern/cbuf.c
index 17369afe..ea848c56 100644
--- a/kern/cbuf.c
+++ b/kern/cbuf.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015 Richard Braun.
+ * Copyright (c) 2015-2017 Richard Braun.
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -15,16 +15,19 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <kern/assert.h>
+#include <assert.h>
+#include <stddef.h>
+#include <string.h>
+
 #include <kern/cbuf.h>
 #include <kern/error.h>
 #include <kern/macros.h>
 
 /* Negative close to 0 so that an overflow occurs early */
-#define CBUF_INIT_INDEX ((unsigned long)-500)
+#define CBUF_INIT_INDEX ((size_t)-500)
 
 void
-cbuf_init(struct cbuf *cbuf, char *buf, unsigned long capacity)
+cbuf_init(struct cbuf *cbuf, char *buf, size_t capacity)
 {
     assert(ISP2(capacity));
 
@@ -34,24 +37,29 @@ cbuf_init(struct cbuf *cbuf, char *buf, unsigned long capacity)
     cbuf->end = cbuf->start;
 }
 
-static unsigned long
-cbuf_index(const struct cbuf *cbuf, unsigned long abs_index)
+static size_t
+cbuf_index(const struct cbuf *cbuf, size_t abs_index)
 {
     return abs_index & (cbuf->capacity - 1);
 }
 
-void
-cbuf_push(struct cbuf *cbuf, char byte)
+static void
+cbuf_update_start(struct cbuf *cbuf)
 {
-    cbuf->buf[cbuf_index(cbuf, cbuf->end)] = byte;
-    cbuf->end++;
-
     /* Mind integer overflows */
     if (cbuf_size(cbuf) > cbuf->capacity) {
         cbuf->start = cbuf->end - cbuf->capacity;
     }
 }
 
+void
+cbuf_push(struct cbuf *cbuf, char byte)
+{
+    cbuf->buf[cbuf_index(cbuf, cbuf->end)] = byte;
+    cbuf->end++;
+    cbuf_update_start(cbuf);
+}
+
 int
 cbuf_pop(struct cbuf *cbuf, char *bytep)
 {
@@ -65,13 +73,76 @@ cbuf_pop(struct cbuf *cbuf, char *bytep)
 }
 
 int
-cbuf_read(const struct cbuf *cbuf, unsigned long index, char *bytep)
+cbuf_write(struct cbuf *cbuf, size_t index, const void *buf, size_t size)
 {
-    /* Mind integer overflows */
-    if ((cbuf->end - index - 1) >= cbuf_size(cbuf)) {
+    char *start, *end, *buf_end;
+    size_t new_end, skip;
+
+    if (!cbuf_range_valid(cbuf, index, cbuf->end)) {
         return ERROR_INVAL;
     }
 
-    *bytep = cbuf->buf[cbuf_index(cbuf, index)];
+    new_end = index + size;
+
+    if (!cbuf_range_valid(cbuf, cbuf->start, new_end)) {
+        cbuf->end = new_end;
+
+        if (size > cbuf_capacity(cbuf)) {
+            skip = size - cbuf_capacity(cbuf);
+            buf += skip;
+            index += skip;
+            size = cbuf_capacity(cbuf);
+        }
+    }
+
+    start = &cbuf->buf[cbuf_index(cbuf, index)];
+    end = start + size;
+    buf_end = cbuf->buf + cbuf->capacity;
+
+    if ((end <= cbuf->buf) || (end > buf_end)) {
+        skip = buf_end - start;
+        memcpy(start, buf, skip);
+        buf += skip;
+        start = cbuf->buf;
+        size -= skip;
+    }
+
+    memcpy(start, buf, size);
+    cbuf_update_start(cbuf);
+    return 0;
+}
+
+int
+cbuf_read(const struct cbuf *cbuf, size_t index, void *buf, size_t *sizep)
+{
+    const char *start, *end, *buf_end;
+    size_t size;
+
+    /* At least one byte must be available */
+    if (!cbuf_range_valid(cbuf, index, index + 1)) {
+        return ERROR_INVAL;
+    }
+
+    size = cbuf->end - index;
+
+    if (*sizep > size) {
+        *sizep = size;
+    }
+
+    start = &cbuf->buf[cbuf_index(cbuf, index)];
+    end = start + *sizep;
+    buf_end = cbuf->buf + cbuf->capacity;
+
+    if ((end > cbuf->buf) && (end <= buf_end)) {
+        size = *sizep;
+    } else {
+        size = buf_end - start;
+        memcpy(buf, start, size);
+        buf += size;
+        start = cbuf->buf;
+        size = *sizep - size;
+    }
+
+    memcpy(buf, start, size);
     return 0;
 }