diff --git a/include/lib/cbuf.h b/include/lib/cbuf.h new file mode 100644 index 000000000..d71e308f6 --- /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 +#include + +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 000000000..1ab8ae0c9 --- /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 +#include + +/* 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 000000000..4c3129544 --- /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 +#include +#include +#include +#include +#include + +#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 000000000..2c57d4813 --- /dev/null +++ b/lib/cbuf/rules.mk @@ -0,0 +1,4 @@ +LOCAL_DIR := $(GET_LOCAL_DIR) + +OBJS += \ + $(LOCAL_DIR)/cbuf.o