initial revision
[ucgine.git] / lib / cirbuf / ucg_cirbuf.c
1 /*
2  * Copyright (c) 2009-2015, Olivier MATZ <zer0@droids-corp.org>
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions are met:
6  *
7  *     * Redistributions of source code must retain the above copyright
8  *       notice, this list of conditions and the following disclaimer.
9  *     * Redistributions in binary form must reproduce the above copyright
10  *       notice, this list of conditions and the following disclaimer in the
11  *       documentation and/or other materials provided with the distribution.
12  *     * Neither the name of the University of California, Berkeley nor the
13  *       names of its contributors may be used to endorse or promote products
14  *       derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
17  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
20  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  */
27
28 /*-
29  * Copyright (c) <2010>, Intel Corporation
30  * All rights reserved.
31  *
32  * Redistribution and use in source and binary forms, with or without
33  * modification, are permitted provided that the following conditions
34  * are met:
35  *
36  * - Redistributions of source code must retain the above copyright
37  *   notice, this list of conditions and the following disclaimer.
38  *
39  * - Redistributions in binary form must reproduce the above copyright
40  *   notice, this list of conditions and the following disclaimer in
41  *   the documentation and/or other materials provided with the
42  *   distribution.
43  *
44  * - Neither the name of Intel Corporation nor the names of its
45  *   contributors may be used to endorse or promote products derived
46  *   from this software without specific prior written permission.
47  *
48  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
49  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
50  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
51  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
52  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
53  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
54  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
55  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
56  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
57  * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
58  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
59  * OF THE POSSIBILITY OF SUCH DAMAGE.
60  */
61
62 #include <string.h>
63 #include <errno.h>
64
65 #include <ucg_cirbuf.h>
66
67 /* init a circular buffer */
68 void
69 ucg_cirbuf_init(struct ucg_cirbuf *cbuf, char *buf, unsigned start,
70         unsigned maxlen)
71 {
72         cbuf->maxlen = maxlen;
73         cbuf->len = 0;
74         cbuf->start = start;
75         cbuf->buf = buf;
76 }
77
78 /* return the index of the tail (contains an empty element) */
79 static unsigned
80 ucg_cirbuf_get_end(const struct ucg_cirbuf *cbuf)
81 {
82         unsigned end;
83
84         end = cbuf->start + cbuf->len;
85         if (end >= cbuf->maxlen)
86                 end -= cbuf->maxlen;
87
88         return end;
89 }
90
91 /* multiple add at head */
92 int
93 ucg_cirbuf_add_buf_head(struct ucg_cirbuf *cbuf, const char *buf, unsigned n)
94 {
95         unsigned remain = n;
96         int copy_start;
97         unsigned copy_len;
98
99         if (remain == 0 || remain > ucg_cirbuf_get_freelen(cbuf))
100                 return -EINVAL;
101
102         copy_start = cbuf->start - remain;
103         if (copy_start < 0)
104                 copy_start += cbuf->maxlen;
105         cbuf->start = copy_start;
106         cbuf->len += remain;
107         copy_len = remain;
108         if ((unsigned)copy_start + copy_len >= cbuf->maxlen)
109                 copy_len = cbuf->maxlen - copy_start;
110         if (copy_len > 0) {
111                 memcpy(cbuf->buf + copy_start, buf, copy_len);
112                 buf += copy_len;
113                 remain -= copy_len;
114         }
115         if (remain == 0)
116                 return n;
117
118         copy_len = remain;
119         memcpy(cbuf->buf, buf, copy_len);
120
121         return n;
122 }
123
124 /* multiple add at tail */
125 int
126 ucg_cirbuf_add_buf_tail(struct ucg_cirbuf *cbuf, const char *buf, unsigned n)
127 {
128         unsigned remain = n;
129         unsigned copy_start, copy_len;
130
131         if (remain == 0 || remain > ucg_cirbuf_get_freelen(cbuf))
132                 return -EINVAL;
133
134         copy_start = ucg_cirbuf_get_end(cbuf);
135         cbuf->len += remain;
136         copy_len = remain;
137         if (copy_start + copy_len >= cbuf->maxlen)
138                 copy_len = cbuf->maxlen - copy_start;
139         if (copy_len > 0) {
140                 memcpy(cbuf->buf + copy_start, buf, copy_len);
141                 buf += copy_len;
142                 remain -= copy_len;
143         }
144         if (remain == 0)
145                 return n;
146
147         copy_len = remain;
148         memcpy(cbuf->buf, buf, copy_len);
149
150         return n;
151 }
152
153 /* single add at head */
154 static inline void
155 __cirbuf_add_head(struct ucg_cirbuf *cbuf, char c)
156 {
157         unsigned start = cbuf->start;
158
159         if (start == 0)
160                 start = cbuf->maxlen - 1;
161         else
162                 start = start - 1;
163         cbuf->buf[start] = c;
164         cbuf->start = start;
165         cbuf->len++;
166 }
167
168 /* single add at head, checking if full first */
169 int
170 ucg_cirbuf_add_head_safe(struct ucg_cirbuf *cbuf, char c)
171 {
172         if (!ucg_cirbuf_is_full(cbuf)) {
173                 __cirbuf_add_head(cbuf, c);
174                 return 0;
175         }
176         return -EINVAL;
177 }
178
179 /* single add at head */
180 void
181 ucg_cirbuf_add_head(struct ucg_cirbuf *cbuf, char c)
182 {
183         __cirbuf_add_head(cbuf, c);
184 }
185
186 /* single add at tail */
187 static inline void
188 __cirbuf_add_tail(struct ucg_cirbuf *cbuf, char c)
189 {
190         unsigned end = ucg_cirbuf_get_end(cbuf);
191
192         cbuf->buf[end] = c;
193         cbuf->len++;
194 }
195
196 /* single add at tail, checking if full first */
197 int
198 ucg_cirbuf_add_tail_safe(struct ucg_cirbuf *cbuf, char c)
199 {
200         if (!ucg_cirbuf_is_full(cbuf)) {
201                 __cirbuf_add_tail(cbuf, c);
202                 return 0;
203         }
204         return -EINVAL;
205 }
206
207 /* single add at tail */
208 void
209 ucg_cirbuf_add_tail(struct ucg_cirbuf *cbuf, char c)
210 {
211         __cirbuf_add_tail(cbuf, c);
212 }
213
214 /* multiple delete at head */
215 int
216 ucg_cirbuf_del_buf_head(struct ucg_cirbuf *cbuf, unsigned size)
217 {
218         if (size == 0 || size > ucg_cirbuf_get_len(cbuf))
219                 return -EINVAL;
220
221         cbuf->len -= size;
222         if (ucg_cirbuf_is_empty(cbuf)) {
223                 cbuf->start += size - 1;
224                 cbuf->start %= cbuf->maxlen;
225         }
226         else {
227                 cbuf->start += size;
228                 cbuf->start %= cbuf->maxlen;
229         }
230         return 0;
231 }
232
233 /* multiple delete at tail */
234 int
235 ucg_cirbuf_del_buf_tail(struct ucg_cirbuf *cbuf, unsigned size)
236 {
237         if (size == 0 || size > ucg_cirbuf_get_len(cbuf))
238                 return -EINVAL;
239
240         cbuf->len -= size;
241         return 0;
242 }
243
244 /* single del at head */
245 static inline void
246 __cirbuf_del_head(struct ucg_cirbuf *cbuf)
247 {
248         cbuf->len --;
249         if (!ucg_cirbuf_is_empty(cbuf)) {
250                 cbuf->start ++;
251                 cbuf->start %= cbuf->maxlen;
252         }
253 }
254
255 /* single del at head, checking if empty first */
256 int
257 ucg_cirbuf_del_head_safe(struct ucg_cirbuf *cbuf)
258 {
259         if (cbuf && !ucg_cirbuf_is_empty(cbuf)) {
260                 __cirbuf_del_head(cbuf);
261                 return 0;
262         }
263         return -EINVAL;
264 }
265
266 /* single del at head */
267 void
268 ucg_cirbuf_del_head(struct ucg_cirbuf *cbuf)
269 {
270         __cirbuf_del_head(cbuf);
271 }
272
273 /* single del at tail */
274 static inline void
275 __cirbuf_del_tail(struct ucg_cirbuf *cbuf)
276 {
277         cbuf->len--;
278 }
279
280 /* single del at tail, checking if empty first */
281 int
282 ucg_cirbuf_del_tail_safe(struct ucg_cirbuf *cbuf)
283 {
284         if (cbuf && !ucg_cirbuf_is_empty(cbuf)) {
285                 __cirbuf_del_tail(cbuf);
286                 return 0;
287         }
288         return -EINVAL;
289 }
290
291 /* single del at tail */
292 void
293 ucg_cirbuf_del_tail(struct ucg_cirbuf *cbuf)
294 {
295         __cirbuf_del_tail(cbuf);
296 }
297
298 /* convert to buffer */
299 int
300 ucg_cirbuf_get_buf_head(const struct ucg_cirbuf *cbuf, char *buf, unsigned n)
301 {
302         unsigned remain = n;
303         unsigned cirbuf_len, copy_start, copy_len;
304
305         cirbuf_len = ucg_cirbuf_get_len(cbuf);
306         if (remain >= cirbuf_len)
307                 remain = cirbuf_len;
308
309         if (remain == 0)
310                 return 0;
311
312         copy_start = cbuf->start;
313         copy_len = remain;
314         if (copy_start + copy_len >= cbuf->maxlen)
315                 copy_len = cbuf->maxlen - copy_start;
316         if (copy_len > 0) {
317                 memcpy(buf, cbuf->buf + copy_start, copy_len);
318                 buf += copy_len;
319                 remain -= copy_len;
320         }
321         if (remain == 0)
322                 return n;
323
324         copy_len = remain;
325         memcpy(buf, cbuf->buf, copy_len);
326
327         return n;
328 }
329
330 /* convert to buffer */
331 int
332 ucg_cirbuf_get_buf_tail(const struct ucg_cirbuf *cbuf, char *buf, unsigned n)
333 {
334         unsigned remain = n;
335         int copy_start;
336         unsigned cirbuf_len, copy_len;
337
338         cirbuf_len = ucg_cirbuf_get_len(cbuf);
339         if (remain >= cirbuf_len)
340                 remain = cirbuf_len;
341
342         if (remain == 0)
343                 return 0;
344
345         copy_start = ucg_cirbuf_get_end(cbuf) - remain;
346         if (copy_start < 0)
347                 copy_start += cbuf->maxlen;
348         copy_len = remain;
349         if ((unsigned)copy_start + copy_len >= cbuf->maxlen)
350                 copy_len = cbuf->maxlen - copy_start;
351         if (copy_len > 0) {
352                 memcpy(buf, cbuf->buf + copy_start, copy_len);
353                 buf += copy_len;
354                 remain -= copy_len;
355         }
356         if (remain == 0)
357                 return n;
358
359         copy_len = remain;
360         memcpy(buf, cbuf->buf, copy_len);
361
362         return n;
363 }
364
365 /* get head */
366 char
367 ucg_cirbuf_get_head(const struct ucg_cirbuf *cbuf)
368 {
369         return cbuf->buf[cbuf->start];
370 }
371
372 /* get tail */
373 char
374 ucg_cirbuf_get_tail(const struct ucg_cirbuf *cbuf)
375 {
376         unsigned end;
377
378         /* should not happen */
379         if (cbuf->len == 0)
380                 return 0;
381
382         end = cbuf->start + cbuf->len - 1;
383         if (end >= cbuf->maxlen)
384                 end -= cbuf->maxlen;
385         return cbuf->buf[end];
386 }
387
388 static void
389 __ucg_cirbuf_shift(struct ucg_cirbuf *cbuf, unsigned n)
390 {
391         char tmp, tmp2;
392         unsigned start, cur, min;
393
394         start = 0;
395         cur = 0;
396         tmp = cbuf->buf[0];
397         min = cbuf->maxlen;
398
399         while (1) {
400                 cur = cur + n;
401                 if (cur >= cbuf->maxlen)
402                         cur -= cbuf->maxlen;
403                 tmp2 = cbuf->buf[cur];
404                 cbuf->buf[cur] = tmp;
405                 tmp = tmp2;
406                 if (cur == start) {
407                         if ((cur + 1) == min)
408                                 break;
409                         cur++;
410                         tmp = cbuf->buf[cur];
411                         start = cur;
412                 } else if (cur < min) {
413                         min = cur;
414                 }
415         }
416
417         cbuf->start += n;
418         if (cbuf->start >= cbuf->maxlen)
419                 cbuf->start -= cbuf->maxlen;
420 }
421
422 void ucg_cirbuf_align_left(struct ucg_cirbuf *cbuf)
423 {
424         __ucg_cirbuf_shift(cbuf, cbuf->maxlen - cbuf->start);
425 }
426
427 void ucg_cirbuf_align_right(struct ucg_cirbuf *cbuf)
428 {
429         unsigned end = ucg_cirbuf_get_end(cbuf);
430         __ucg_cirbuf_shift(cbuf, cbuf->maxlen - end);
431 }
432
433 #ifdef TEST_CIRBUF
434 #include <stdint.h>
435 #include <assert.h>
436
437 void dump_it(struct ucg_cirbuf * cbuf)
438 {
439         unsigned i;
440         int idx;
441         char e;
442
443         printf("sta=%2.2d len=%2.2d/%2.2d { ",
444                 cbuf->start,
445                 ucg_cirbuf_get_len(cbuf),
446                 ucg_cirbuf_get_maxlen(cbuf));
447
448         for (i = 0; i < ucg_cirbuf_get_maxlen(cbuf); i++) {
449                 idx = i - cbuf->start;
450                 if (idx < 0)
451                         idx += cbuf->maxlen;
452                 if (idx < (int)ucg_cirbuf_get_len(cbuf))
453                         printf("%2.2x, ", cbuf->buf[i] & 0xFF);
454                 else
455                         printf("XX, ");
456         }
457         printf("}  -> ");
458
459         printf("[ ");
460         UCG_CIRBUF_FOREACH(cbuf, i, e) {
461                 printf("%2.2x, ", e & 0xFF);
462         }
463         printf("]\n");
464 }
465
466 int main(void)
467 {
468         unsigned i;
469         struct ucg_cirbuf my_fifo;
470         char fifo_buf[16];
471
472         char buf1[] = { 0x10, 0x11, 0x12 };
473         char buf2[] = { 0x20, 0x21, 0x22, 0x23 };
474
475         char tmp_buf[16];
476         char ref_buf[] = { 0x20, 0x21, 0x22, 0x23, 0x01, 0x10, 0x11, 0x12 };
477
478         /* Test 1 */
479
480         printf("Test 1\n");
481
482         ucg_cirbuf_init(&my_fifo, fifo_buf, 0, 4);
483         assert(ucg_cirbuf_is_empty(&my_fifo));
484         assert(!ucg_cirbuf_is_full(&my_fifo));
485         dump_it(&my_fifo);
486
487         ucg_cirbuf_add_tail(&my_fifo, 1);
488         assert(ucg_cirbuf_get_head(&my_fifo) == 1);
489         assert(ucg_cirbuf_get_tail(&my_fifo) == 1);
490         dump_it(&my_fifo);
491
492
493         ucg_cirbuf_add_tail(&my_fifo, 2);
494         assert(!ucg_cirbuf_is_empty(&my_fifo));
495         assert(!ucg_cirbuf_is_full(&my_fifo));
496         dump_it(&my_fifo);
497
498         ucg_cirbuf_add_tail(&my_fifo, 3);
499         assert(ucg_cirbuf_get_head(&my_fifo) == 1);
500         assert(ucg_cirbuf_get_tail(&my_fifo) == 3);
501         dump_it(&my_fifo);
502
503         ucg_cirbuf_add_tail(&my_fifo, 4);
504         assert(!ucg_cirbuf_is_empty(&my_fifo));
505         assert(ucg_cirbuf_is_full(&my_fifo));
506         dump_it(&my_fifo);
507
508         ucg_cirbuf_del_tail(&my_fifo);
509         dump_it(&my_fifo);
510         assert(ucg_cirbuf_get_tail(&my_fifo) == 3);
511         assert(ucg_cirbuf_get_head(&my_fifo) == 1);
512
513         ucg_cirbuf_del_head(&my_fifo);
514         assert(ucg_cirbuf_get_tail(&my_fifo) == 3);
515         assert(ucg_cirbuf_get_head(&my_fifo) == 2);
516         dump_it(&my_fifo);
517
518         ucg_cirbuf_del_head(&my_fifo);
519         assert(ucg_cirbuf_get_tail(&my_fifo) == 3);
520         assert(ucg_cirbuf_get_head(&my_fifo) == 3);
521         dump_it(&my_fifo);
522
523         ucg_cirbuf_del_head(&my_fifo);
524         assert(ucg_cirbuf_is_empty(&my_fifo));
525         dump_it(&my_fifo);
526
527
528         /* Test 2 */
529
530         printf("Test 2\n");
531
532         ucg_cirbuf_init(&my_fifo, fifo_buf, 2, 4);
533         dump_it(&my_fifo);
534
535         ucg_cirbuf_add_head(&my_fifo, 4);
536         assert(ucg_cirbuf_get_head(&my_fifo) == 4);
537         assert(ucg_cirbuf_get_tail(&my_fifo) == 4);
538         dump_it(&my_fifo);
539
540
541         ucg_cirbuf_add_head(&my_fifo, 3);
542         assert(!ucg_cirbuf_is_empty(&my_fifo));
543         assert(!ucg_cirbuf_is_full(&my_fifo));
544         dump_it(&my_fifo);
545
546         ucg_cirbuf_add_head(&my_fifo, 2);
547         assert(ucg_cirbuf_get_head(&my_fifo) == 2);
548         assert(ucg_cirbuf_get_tail(&my_fifo) == 4);
549         dump_it(&my_fifo);
550
551         ucg_cirbuf_add_head(&my_fifo, 1);
552         assert(!ucg_cirbuf_is_empty(&my_fifo));
553         assert(ucg_cirbuf_is_full(&my_fifo));
554         dump_it(&my_fifo);
555
556
557         /* Test 3 */
558
559         printf("Test 3\n");
560
561         for (i = 0; i < 16; i++) {
562                 ucg_cirbuf_init(&my_fifo, fifo_buf, i, 16);
563                 dump_it(&my_fifo);
564                 ucg_cirbuf_add_buf_head(&my_fifo, buf1, sizeof(buf1));
565                 dump_it(&my_fifo);
566                 ucg_cirbuf_add_head(&my_fifo, 1);
567                 dump_it(&my_fifo);
568                 ucg_cirbuf_add_buf_head(&my_fifo, buf2, sizeof(buf2));
569                 dump_it(&my_fifo);
570                 ucg_cirbuf_get_buf_head(&my_fifo, tmp_buf, sizeof(tmp_buf));
571                 assert(memcmp(tmp_buf, ref_buf, sizeof(ref_buf)) == 0);
572         }
573
574         /* Test 4 */
575
576         printf("Test 4\n");
577
578         for (i = 0; i < 16; i++) {
579                 ucg_cirbuf_init(&my_fifo, fifo_buf, i, 16);
580                 dump_it(&my_fifo);
581                 ucg_cirbuf_add_buf_tail(&my_fifo, buf2, sizeof(buf2));
582                 dump_it(&my_fifo);
583                 ucg_cirbuf_add_tail(&my_fifo, 1);
584                 dump_it(&my_fifo);
585                 ucg_cirbuf_add_buf_tail(&my_fifo, buf1, sizeof(buf1));
586                 dump_it(&my_fifo);
587                 ucg_cirbuf_get_buf_tail(&my_fifo, tmp_buf, sizeof(tmp_buf));
588                 assert(memcmp(tmp_buf, ref_buf, sizeof(ref_buf)) == 0);
589
590                 printf("align left\n");
591                 ucg_cirbuf_align_left(&my_fifo);
592                 dump_it(&my_fifo);
593                 ucg_cirbuf_get_buf_tail(&my_fifo, tmp_buf, sizeof(tmp_buf));
594                 assert(memcmp(tmp_buf, ref_buf, sizeof(ref_buf)) == 0);
595                 assert(my_fifo.start == 0);
596
597                 printf("align right\n");
598                 ucg_cirbuf_align_right(&my_fifo);
599                 dump_it(&my_fifo);
600                 ucg_cirbuf_get_buf_tail(&my_fifo, tmp_buf, sizeof(tmp_buf));
601                 assert(memcmp(tmp_buf, ref_buf, sizeof(ref_buf)) == 0);
602                 assert(my_fifo.start + my_fifo.len == my_fifo.maxlen);
603         }
604
605         /* Test 5 */
606
607         printf("Test 5\n");
608
609         ucg_cirbuf_init(&my_fifo, fifo_buf, 10, 16);
610         dump_it(&my_fifo);
611         i = 0;
612         while (ucg_cirbuf_add_tail_safe(&my_fifo, i) == 0)
613                 i++;
614         dump_it(&my_fifo);
615         ucg_cirbuf_del_buf_tail(&my_fifo, 10);
616         dump_it(&my_fifo);
617         assert(ucg_cirbuf_get_len(&my_fifo) == 6);
618         assert(ucg_cirbuf_del_buf_tail(&my_fifo, 10) != 0);
619         assert(ucg_cirbuf_get_tail(&my_fifo) == 5);
620         assert(ucg_cirbuf_get_head(&my_fifo) == 0);
621
622         return 0;
623 }
624 #endif