1 /*
2 SFIFO 1.3 Simple portable lock-free FIFO
3
4 (c) 2000-2002, David Olofson - free software under the terms of the LGPL 2.1
5 */
6
7
8 /*
9 -----------------------------------------------------------
10 TODO:
11 * Is there a way to avoid losing one byte of buffer
12 space to avoid extra variables or locking?
13
14 * Test more compilers and environments.
15 -----------------------------------------------------------
16 */
17
18 #include <string.h>
19 #include <stdlib.h>
20
21 #include "sfifo.h"
22 #include "debug.h"
23
24 /*
25 * Alloc buffer, init FIFO etc...
26 */
sfifo_init(sfifo_t * f,int size)27 SFIFO_SCOPE int sfifo_init(sfifo_t *f, int size)
28 {
29 memset(f, 0, sizeof(sfifo_t));
30
31 if(size > SFIFO_MAX_BUFFER_SIZE)
32 return -EINVAL;
33
34 /*
35 * Set sufficient power-of-2 size.
36 *
37 * No, there's no bug. If you need
38 * room for N bytes, the buffer must
39 * be at least N+1 bytes. (The fifo
40 * can't tell 'empty' from 'full'
41 * without unsafe index manipulations
42 * otherwise.)
43 */
44 f->size = 1;
45 for(; f->size <= size; f->size <<= 1)
46 ;
47
48 /* Get buffer */
49 if( 0 == (f->buffer = (void *)malloc(f->size)) )
50 return -ENOMEM;
51
52 return 0;
53 }
54
55 /*
56 * Dealloc buffer etc...
57 */
sfifo_close(sfifo_t * f)58 SFIFO_SCOPE void sfifo_close(sfifo_t *f)
59 {
60 if(f->buffer) {
61 free(f->buffer);
62 f->buffer = NULL; /* Prevent double free */
63 }
64 }
65
66 /*
67 * Empty FIFO buffer
68 */
sfifo_flush(sfifo_t * f)69 SFIFO_SCOPE void sfifo_flush(sfifo_t *f)
70 {
71 debug("sfifo_flush()");
72 /* Reset positions */
73 f->readpos = 0;
74 f->writepos = 0;
75 }
76
77 /*
78 * Write bytes to a FIFO
79 * Return number of bytes written, or an error code
80 */
sfifo_write(sfifo_t * f,const void * _buf,int len)81 SFIFO_SCOPE int sfifo_write(sfifo_t *f, const void *_buf, int len)
82 {
83 int total;
84 int i;
85 const char *buf = (const char *)_buf;
86
87 if(!f->buffer)
88 return -ENODEV; /* No buffer! */
89
90 /* total = len = min(space, len) */
91 total = sfifo_space(f);
92 debug1("sfifo_space() = %d",total);
93 if(len > total)
94 len = total;
95 else
96 total = len;
97 debug1("sfifo_write() = %d", total);
98
99 i = f->writepos;
100 if(i + len > f->size)
101 {
102 memcpy(f->buffer + i, buf, f->size - i);
103 buf += f->size - i;
104 len -= f->size - i;
105 i = 0;
106 }
107 memcpy(f->buffer + i, buf, len);
108 f->writepos = i + len;
109
110 return total;
111 }
112
113
114 /*
115 * Read bytes from a FIFO
116 * Return number of bytes read, or an error code
117 */
sfifo_read(sfifo_t * f,void * _buf,int len)118 SFIFO_SCOPE int sfifo_read(sfifo_t *f, void *_buf, int len)
119 {
120 int total;
121 int i;
122 char *buf = (char *)_buf;
123
124 if(!f->buffer)
125 return -ENODEV; /* No buffer! */
126
127 /* total = len = min(used, len) */
128 total = sfifo_used(f);
129 debug1("sfifo_used() = %d",total);
130 if(len > total)
131 len = total;
132 else
133 total = len;
134 debug1("sfifo_read() = %d", total);
135
136 i = f->readpos;
137 if(i + len > f->size)
138 {
139 memcpy(buf, f->buffer + i, f->size - i);
140 buf += f->size - i;
141 len -= f->size - i;
142 i = 0;
143 }
144 memcpy(buf, f->buffer + i, len);
145 f->readpos = i + len;
146
147 return total;
148 }
149
150