[lib][cbuf] simple circular buffer implementation

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
+

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
+

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;
+}
+

lib/cbuf/rules.mk

@@ -0,0 +1,4 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+OBJS += \
+	$(LOCAL_DIR)/cbuf.o