1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29#include <linux/errno.h>
30#include <linux/kernel.h>
31#include <linux/module.h>
32#include <linux/sched.h>
33#include <linux/string.h>
34#include <asm/uaccess.h>
35
36#include "dvb_ringbuffer.h"
37
38#define PKT_READY 0
39#define PKT_DISPOSED 1
40
41
42void dvb_ringbuffer_init(struct dvb_ringbuffer *rbuf, void *data, size_t len)
43{
44 rbuf->pread=rbuf->pwrite=0;
45 rbuf->data=data;
46 rbuf->size=len;
47 rbuf->error=0;
48
49 init_waitqueue_head(&rbuf->queue);
50
51 spin_lock_init(&(rbuf->lock));
52}
53
54
55
56int dvb_ringbuffer_empty(struct dvb_ringbuffer *rbuf)
57{
58 return (rbuf->pread==rbuf->pwrite);
59}
60
61
62
63ssize_t dvb_ringbuffer_free(struct dvb_ringbuffer *rbuf)
64{
65 ssize_t free;
66
67 free = rbuf->pread - rbuf->pwrite;
68 if (free <= 0)
69 free += rbuf->size;
70 return free-1;
71}
72
73
74
75ssize_t dvb_ringbuffer_avail(struct dvb_ringbuffer *rbuf)
76{
77 ssize_t avail;
78
79 avail = rbuf->pwrite - rbuf->pread;
80 if (avail < 0)
81 avail += rbuf->size;
82 return avail;
83}
84
85
86
87void dvb_ringbuffer_flush(struct dvb_ringbuffer *rbuf)
88{
89 rbuf->pread = rbuf->pwrite;
90 rbuf->error = 0;
91}
92EXPORT_SYMBOL(dvb_ringbuffer_flush);
93
94void dvb_ringbuffer_reset(struct dvb_ringbuffer *rbuf)
95{
96 rbuf->pread = rbuf->pwrite = 0;
97 rbuf->error = 0;
98}
99
100void dvb_ringbuffer_flush_spinlock_wakeup(struct dvb_ringbuffer *rbuf)
101{
102 unsigned long flags;
103
104 spin_lock_irqsave(&rbuf->lock, flags);
105 dvb_ringbuffer_flush(rbuf);
106 spin_unlock_irqrestore(&rbuf->lock, flags);
107
108 wake_up(&rbuf->queue);
109}
110
111ssize_t dvb_ringbuffer_read_user(struct dvb_ringbuffer *rbuf, u8 __user *buf, size_t len)
112{
113 size_t todo = len;
114 size_t split;
115
116 split = (rbuf->pread + len > rbuf->size) ? rbuf->size - rbuf->pread : 0;
117 if (split > 0) {
118 if (copy_to_user(buf, rbuf->data+rbuf->pread, split))
119 return -EFAULT;
120 buf += split;
121 todo -= split;
122 rbuf->pread = 0;
123 }
124 if (copy_to_user(buf, rbuf->data+rbuf->pread, todo))
125 return -EFAULT;
126
127 rbuf->pread = (rbuf->pread + todo) % rbuf->size;
128
129 return len;
130}
131
132void dvb_ringbuffer_read(struct dvb_ringbuffer *rbuf, u8 *buf, size_t len)
133{
134 size_t todo = len;
135 size_t split;
136
137 split = (rbuf->pread + len > rbuf->size) ? rbuf->size - rbuf->pread : 0;
138 if (split > 0) {
139 memcpy(buf, rbuf->data+rbuf->pread, split);
140 buf += split;
141 todo -= split;
142 rbuf->pread = 0;
143 }
144 memcpy(buf, rbuf->data+rbuf->pread, todo);
145
146 rbuf->pread = (rbuf->pread + todo) % rbuf->size;
147}
148
149
150ssize_t dvb_ringbuffer_write(struct dvb_ringbuffer *rbuf, const u8 *buf, size_t len)
151{
152 size_t todo = len;
153 size_t split;
154
155 split = (rbuf->pwrite + len > rbuf->size) ? rbuf->size - rbuf->pwrite : 0;
156
157 if (split > 0) {
158 memcpy(rbuf->data+rbuf->pwrite, buf, split);
159 buf += split;
160 todo -= split;
161 rbuf->pwrite = 0;
162 }
163 memcpy(rbuf->data+rbuf->pwrite, buf, todo);
164 rbuf->pwrite = (rbuf->pwrite + todo) % rbuf->size;
165
166 return len;
167}
168
169ssize_t dvb_ringbuffer_write_user(struct dvb_ringbuffer *rbuf,
170 const u8 __user *buf, size_t len)
171{
172 int status;
173 size_t todo = len;
174 size_t split;
175
176 split = (rbuf->pwrite + len > rbuf->size) ? rbuf->size - rbuf->pwrite : 0;
177
178 if (split > 0) {
179 status = copy_from_user(rbuf->data+rbuf->pwrite, buf, split);
180 if (status)
181 return len - todo;
182 buf += split;
183 todo -= split;
184 rbuf->pwrite = 0;
185 }
186 status = copy_from_user(rbuf->data+rbuf->pwrite, buf, todo);
187 if (status)
188 return len - todo;
189 rbuf->pwrite = (rbuf->pwrite + todo) % rbuf->size;
190
191 return len;
192}
193
194ssize_t dvb_ringbuffer_pkt_write(struct dvb_ringbuffer *rbuf, u8* buf, size_t len)
195{
196 int status;
197 ssize_t oldpwrite = rbuf->pwrite;
198
199 DVB_RINGBUFFER_WRITE_BYTE(rbuf, len >> 8);
200 DVB_RINGBUFFER_WRITE_BYTE(rbuf, len & 0xff);
201 DVB_RINGBUFFER_WRITE_BYTE(rbuf, PKT_READY);
202 status = dvb_ringbuffer_write(rbuf, buf, len);
203
204 if (status < 0) rbuf->pwrite = oldpwrite;
205 return status;
206}
207
208ssize_t dvb_ringbuffer_pkt_read_user(struct dvb_ringbuffer *rbuf, size_t idx,
209 int offset, u8 __user *buf, size_t len)
210{
211 size_t todo;
212 size_t split;
213 size_t pktlen;
214
215 pktlen = rbuf->data[idx] << 8;
216 pktlen |= rbuf->data[(idx + 1) % rbuf->size];
217 if (offset > pktlen) return -EINVAL;
218 if ((offset + len) > pktlen) len = pktlen - offset;
219
220 idx = (idx + DVB_RINGBUFFER_PKTHDRSIZE + offset) % rbuf->size;
221 todo = len;
222 split = ((idx + len) > rbuf->size) ? rbuf->size - idx : 0;
223 if (split > 0) {
224 if (copy_to_user(buf, rbuf->data+idx, split))
225 return -EFAULT;
226 buf += split;
227 todo -= split;
228 idx = 0;
229 }
230 if (copy_to_user(buf, rbuf->data+idx, todo))
231 return -EFAULT;
232
233 return len;
234}
235
236ssize_t dvb_ringbuffer_pkt_read(struct dvb_ringbuffer *rbuf, size_t idx,
237 int offset, u8* buf, size_t len)
238{
239 size_t todo;
240 size_t split;
241 size_t pktlen;
242
243 pktlen = rbuf->data[idx] << 8;
244 pktlen |= rbuf->data[(idx + 1) % rbuf->size];
245 if (offset > pktlen) return -EINVAL;
246 if ((offset + len) > pktlen) len = pktlen - offset;
247
248 idx = (idx + DVB_RINGBUFFER_PKTHDRSIZE + offset) % rbuf->size;
249 todo = len;
250 split = ((idx + len) > rbuf->size) ? rbuf->size - idx : 0;
251 if (split > 0) {
252 memcpy(buf, rbuf->data+idx, split);
253 buf += split;
254 todo -= split;
255 idx = 0;
256 }
257 memcpy(buf, rbuf->data+idx, todo);
258 return len;
259}
260
261void dvb_ringbuffer_pkt_dispose(struct dvb_ringbuffer *rbuf, size_t idx)
262{
263 size_t pktlen;
264
265 rbuf->data[(idx + 2) % rbuf->size] = PKT_DISPOSED;
266
267
268 while(dvb_ringbuffer_avail(rbuf) > DVB_RINGBUFFER_PKTHDRSIZE) {
269 if (DVB_RINGBUFFER_PEEK(rbuf, 2) == PKT_DISPOSED) {
270 pktlen = DVB_RINGBUFFER_PEEK(rbuf, 0) << 8;
271 pktlen |= DVB_RINGBUFFER_PEEK(rbuf, 1);
272 DVB_RINGBUFFER_SKIP(rbuf, pktlen + DVB_RINGBUFFER_PKTHDRSIZE);
273 } else {
274
275 break;
276 }
277 }
278}
279
280ssize_t dvb_ringbuffer_pkt_next(struct dvb_ringbuffer *rbuf, size_t idx, size_t* pktlen)
281{
282 int consumed;
283 int curpktlen;
284 int curpktstatus;
285
286 if (idx == -1) {
287 idx = rbuf->pread;
288 } else {
289 curpktlen = rbuf->data[idx] << 8;
290 curpktlen |= rbuf->data[(idx + 1) % rbuf->size];
291 idx = (idx + curpktlen + DVB_RINGBUFFER_PKTHDRSIZE) % rbuf->size;
292 }
293
294 consumed = (idx - rbuf->pread) % rbuf->size;
295
296 while((dvb_ringbuffer_avail(rbuf) - consumed) > DVB_RINGBUFFER_PKTHDRSIZE) {
297
298 curpktlen = rbuf->data[idx] << 8;
299 curpktlen |= rbuf->data[(idx + 1) % rbuf->size];
300 curpktstatus = rbuf->data[(idx + 2) % rbuf->size];
301
302 if (curpktstatus == PKT_READY) {
303 *pktlen = curpktlen;
304 return idx;
305 }
306
307 consumed += curpktlen + DVB_RINGBUFFER_PKTHDRSIZE;
308 idx = (idx + curpktlen + DVB_RINGBUFFER_PKTHDRSIZE) % rbuf->size;
309 }
310
311
312 return -1;
313}
314
315
316
317EXPORT_SYMBOL(dvb_ringbuffer_init);
318EXPORT_SYMBOL(dvb_ringbuffer_empty);
319EXPORT_SYMBOL(dvb_ringbuffer_free);
320EXPORT_SYMBOL(dvb_ringbuffer_avail);
321EXPORT_SYMBOL(dvb_ringbuffer_flush_spinlock_wakeup);
322EXPORT_SYMBOL(dvb_ringbuffer_read_user);
323EXPORT_SYMBOL(dvb_ringbuffer_read);
324EXPORT_SYMBOL(dvb_ringbuffer_write);
325EXPORT_SYMBOL(dvb_ringbuffer_write_user);
326
