First version
[3rdparty/ote_partner/tlk.git] / lib / cbuf / cbuf.c
1 /*
2  * Copyright (c) 2008 Travis Geiselbrecht
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining
5  * a copy of this software and associated documentation files
6  * (the "Software"), to deal in the Software without restriction,
7  * including without limitation the rights to use, copy, modify, merge,
8  * publish, distribute, sublicense, and/or sell copies of the Software,
9  * and to permit persons to whom the Software is furnished to do so,
10  * subject to the following conditions:
11  *
12  * The above copyright notice and this permission notice shall be
13  * included in all copies or substantial portions of the Software.
14  *
15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
18  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
19  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
20  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
21  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22  */
23 #include <stdlib.h>
24 #include <debug.h>
25 #include <pow2.h>
26 #include <string.h>
27 #include <assert.h>
28 #include <lib/cbuf.h>
29 #include <kernel/event.h>
30
31 #define LOCAL_TRACE 0
32
33 #define INC_POINTER(cbuf, ptr, inc) \
34         modpow2(((ptr) + (inc)), (cbuf)->len_pow2)
35
36 void cbuf_initialize(cbuf_t *cbuf, size_t len)
37 {
38         DEBUG_ASSERT(cbuf);
39         DEBUG_ASSERT(len > 0);
40         DEBUG_ASSERT(ispow2(len));
41
42         cbuf->head = 0;
43         cbuf->tail = 0;
44         cbuf->len_pow2 = log2(len);
45         cbuf->buf = malloc(len);
46         event_init(&cbuf->event, false, 0);
47
48         LTRACEF("len %zd, len_pow2 %u\n", len, cbuf->len_pow2);
49 }
50
51 size_t cbuf_space_avail(cbuf_t *cbuf)
52 {
53         uint consumed = modpow2((uint)(cbuf->head - cbuf->tail), cbuf->len_pow2);
54         return valpow2(cbuf->len_pow2) - consumed - 1;
55 }
56
57 size_t cbuf_write(cbuf_t *cbuf, const void *_buf, size_t len, bool canreschedule)
58 {
59         const char *buf = (const char *)_buf;
60
61         LTRACEF("len %zd\n", len);
62
63         DEBUG_ASSERT(cbuf);
64         DEBUG_ASSERT(_buf);
65         DEBUG_ASSERT(len < valpow2(cbuf->len_pow2));
66
67         enter_critical_section();
68
69         size_t write_len;
70         size_t pos = 0;
71
72         while (pos < len && cbuf_space_avail(cbuf) > 0) {
73                 if (cbuf->head >= cbuf->tail) {
74                         write_len = MIN(valpow2(cbuf->len_pow2) - cbuf->head, len - pos);
75                 } else {
76                         write_len = MIN(cbuf->tail - cbuf->head - 1, len - pos);
77                 }
78
79                 // if it's full, abort and return how much we've written
80                 if (write_len == 0) {
81                         break;
82                 }
83
84                 memcpy(cbuf->buf + cbuf->head, buf + pos, write_len);
85
86                 cbuf->head = INC_POINTER(cbuf, cbuf->head, write_len);
87                 pos += write_len;
88         }
89
90         if (cbuf->head != cbuf->tail)
91                 event_signal(&cbuf->event, canreschedule);
92
93         exit_critical_section();
94
95         return pos;
96 }
97
98 size_t cbuf_read(cbuf_t *cbuf, void *_buf, size_t buflen, bool block)
99 {
100         size_t ret;
101         char *buf = (char *)_buf;
102
103         DEBUG_ASSERT(cbuf);
104         DEBUG_ASSERT(_buf);
105
106         enter_critical_section();
107
108         if (block)
109                 event_wait(&cbuf->event);
110
111         // see if there's data available
112         if (cbuf->tail != cbuf->head) {
113                 size_t pos = 0;
114
115                 // loop until we've read everything we need
116                 // at most this will make two passes to deal with wraparound
117                 while (pos < buflen && cbuf->tail != cbuf->head) {
118                         size_t read_len;
119                         if (cbuf->head > cbuf->tail) {
120                                 // simple case where there is no wraparound
121                                 read_len = MIN(cbuf->head - cbuf->tail, buflen - pos);
122                         } else {
123                                 // read to the end of buffer in this pass
124                                 read_len = MIN(valpow2(cbuf->len_pow2) - cbuf->tail, buflen - pos);
125                         }
126
127                         memcpy(buf + pos, cbuf->buf + cbuf->tail, read_len);
128
129                         cbuf->tail = INC_POINTER(cbuf, cbuf->tail, read_len);
130                         pos += read_len;
131                 }
132
133                 if (cbuf->tail == cbuf->head) {
134                         // we've emptied the buffer, unsignal the event
135                         event_unsignal(&cbuf->event);
136                 }
137
138                 ret = pos;
139         } else {
140                 DEBUG_ASSERT(!block);
141                 ret = 0;
142         }
143
144         exit_critical_section();
145
146         return ret;
147 }
148