[lib][cbuf] simple circular buffer implementation
diff --git a/include/lib/cbuf.h b/include/lib/cbuf.h
new file mode 100644
index 0000000..d71e308
--- /dev/null
+++ b/include/lib/cbuf.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2009 Travis Geiselbrecht
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#ifndef __LIB_CBUF_H
+#define __LIB_CBUF_H
+
+#include <sys/types.h>
+#include <kernel/event.h>
+
+typedef struct cbuf {
+ uint head;
+ uint tail;
+ uint len_pow2;
+ char *buf;
+ event_t event;
+} cbuf_t;
+
+void cbuf_initialize(cbuf_t *cbuf, size_t len);
+size_t cbuf_read(cbuf_t *cbuf, void *_buf, size_t buflen, bool block);
+size_t cbuf_write(cbuf_t *cbuf, const void *_buf, size_t len, bool canreschedule);
+
+#endif
+
diff --git a/include/pow2.h b/include/pow2.h
new file mode 100644
index 0000000..1ab8ae0
--- /dev/null
+++ b/include/pow2.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2008 Travis Geiselbrecht
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#ifndef __POW2_H
+#define __POW2_H
+
+#include <sys/types.h>
+#include <compiler.h>
+
+/* routines for dealing with power of 2 values for efficiency */
+static inline __ALWAYS_INLINE bool ispow2(uint val)
+{
+ return ((val - 1) & val) == 0;
+}
+
+static inline __ALWAYS_INLINE uint log2(uint val)
+{
+ if (!ispow2(val))
+ return 0; // undefined
+
+ return __builtin_ctz(val);
+}
+
+static inline __ALWAYS_INLINE uint valpow2(uint valp2)
+{
+ return 1 << valp2;
+}
+
+static inline __ALWAYS_INLINE uint divpow2(uint val, uint divp2)
+{
+ return val >> divp2;
+}
+
+static inline __ALWAYS_INLINE uint modpow2(uint val, uint modp2)
+{
+ return val & ((1UL << modp2) - 1);
+}
+
+
+#endif
+
diff --git a/lib/cbuf/cbuf.c b/lib/cbuf/cbuf.c
new file mode 100644
index 0000000..4c31295
--- /dev/null
+++ b/lib/cbuf/cbuf.c
@@ -0,0 +1,146 @@
+/*
+ * Copyright (c) 2008 Travis Geiselbrecht
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#include <stdlib.h>
+#include <debug.h>
+#include <pow2.h>
+#include <string.h>
+#include <lib/cbuf.h>
+#include <kernel/event.h>
+
+#define LOCAL_TRACE 0
+
+#define INC_POINTER(cbuf, ptr, inc) \
+ modpow2(((ptr) + (inc)), (cbuf)->len_pow2)
+
+void cbuf_initialize(cbuf_t *cbuf, size_t len)
+{
+ DEBUG_ASSERT(cbuf);
+ DEBUG_ASSERT(len > 0);
+ DEBUG_ASSERT(ispow2(len));
+
+ cbuf->head = 0;
+ cbuf->tail = 0;
+ cbuf->len_pow2 = log2(len);
+ cbuf->buf = malloc(len);
+ event_init(&cbuf->event, false, 0);
+
+ LTRACEF("len %zd, len_pow2 %u\n", len, cbuf->len_pow2);
+}
+
+static size_t cbuf_space_avail(cbuf_t *cbuf)
+{
+ return (cbuf->head + valpow2(cbuf->len_pow2) - cbuf->tail - 1);
+}
+
+size_t cbuf_write(cbuf_t *cbuf, const void *_buf, size_t len, bool canreschedule)
+{
+ const char *buf = (const char *)_buf;
+
+ LTRACEF("len %zd\n", len);
+
+ DEBUG_ASSERT(cbuf);
+ DEBUG_ASSERT(_buf);
+ DEBUG_ASSERT(len < valpow2(cbuf->len_pow2));
+
+ enter_critical_section();
+
+ size_t write_len;
+ size_t pos = 0;
+
+ while (pos < len && cbuf_space_avail(cbuf) > 0) {
+ if (cbuf->head >= cbuf->tail) {
+ write_len = MIN(valpow2(cbuf->len_pow2) - cbuf->head, len - pos);
+ } else {
+ write_len = MIN(cbuf->tail - cbuf->head - 1, len - pos);
+ }
+
+ // if it's full, abort and return how much we've written
+ if (write_len == 0) {
+ break;
+ }
+
+ memcpy(cbuf->buf + cbuf->head, buf + pos, write_len);
+
+ cbuf->head = INC_POINTER(cbuf, cbuf->head, write_len);
+ pos += write_len;
+ }
+
+ if (cbuf->head != cbuf->tail)
+ event_signal(&cbuf->event, canreschedule);
+
+ exit_critical_section();
+
+ return pos;
+}
+
+size_t cbuf_read(cbuf_t *cbuf, void *_buf, size_t buflen, bool block)
+{
+ size_t ret;
+ char *buf = (char *)_buf;
+
+ DEBUG_ASSERT(cbuf);
+ DEBUG_ASSERT(_buf);
+
+ enter_critical_section();
+
+ if (block)
+ event_wait(&cbuf->event);
+
+ // see if there's data available
+ if (cbuf->tail != cbuf->head) {
+ size_t pos = 0;
+
+ // loop until we've read everything we need
+ // at most this will make two passes to deal with wraparound
+ while (pos < buflen && cbuf->tail != cbuf->head) {
+ size_t read_len;
+ if (cbuf->head > cbuf->tail) {
+ // simple case where there is no wraparound
+ read_len = MIN(cbuf->head - cbuf->tail, buflen - pos);
+ } else {
+ // read to the end of buffer in this pass
+ read_len = MIN(valpow2(cbuf->len_pow2) - cbuf->tail, buflen - pos);
+ }
+
+ memcpy(buf + pos, cbuf->buf + cbuf->tail, read_len);
+
+ cbuf->tail = INC_POINTER(cbuf, cbuf->tail, read_len);
+ pos += read_len;
+ }
+
+ if (cbuf->tail == cbuf->head) {
+ // we've emptied the buffer, unsignal the event
+ event_unsignal(&cbuf->event);
+ }
+
+ ret = pos;
+ } else {
+ DEBUG_ASSERT(!block);
+ ret = 0;
+ }
+
+ exit_critical_section();
+
+ return ret;
+}
+
diff --git a/lib/cbuf/rules.mk b/lib/cbuf/rules.mk
new file mode 100644
index 0000000..2c57d48
--- /dev/null
+++ b/lib/cbuf/rules.mk
@@ -0,0 +1,4 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+OBJS += \
+ $(LOCAL_DIR)/cbuf.o