]>
Commit | Line | Data |
---|---|---|
1 | /* | |
2 | * A generic kernel FIFO implementation | |
3 | * | |
4 | * Copyright (C) 2009/2010 Stefani Seibold <stefani@seibold.net> | |
5 | * | |
6 | * This program is free software; you can redistribute it and/or modify | |
7 | * it under the terms of the GNU General Public License as published by | |
8 | * the Free Software Foundation; either version 2 of the License, or | |
9 | * (at your option) any later version. | |
10 | * | |
11 | * This program is distributed in the hope that it will be useful, | |
12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
14 | * GNU General Public License for more details. | |
15 | * | |
16 | * You should have received a copy of the GNU General Public License | |
17 | * along with this program; if not, write to the Free Software | |
18 | * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. | |
19 | * | |
20 | */ | |
21 | ||
22 | #include <linux/kernel.h> | |
23 | #include <linux/module.h> | |
24 | #include <linux/slab.h> | |
25 | #include <linux/err.h> | |
26 | #include <linux/log2.h> | |
27 | #include <linux/uaccess.h> | |
28 | #include <linux/kfifo.h> | |
29 | ||
30 | /* | |
31 | * internal helper to calculate the unused elements in a fifo | |
32 | */ | |
33 | static inline unsigned int kfifo_unused(struct __kfifo *fifo) | |
34 | { | |
35 | return (fifo->mask + 1) - (fifo->in - fifo->out); | |
36 | } | |
37 | ||
38 | int __kfifo_alloc(struct __kfifo *fifo, unsigned int size, | |
39 | size_t esize, gfp_t gfp_mask) | |
40 | { | |
41 | /* | |
42 | * round down to the next power of 2, since our 'let the indices | |
43 | * wrap' technique works only in this case. | |
44 | */ | |
45 | if (!is_power_of_2(size)) | |
46 | size = rounddown_pow_of_two(size); | |
47 | ||
48 | fifo->in = 0; | |
49 | fifo->out = 0; | |
50 | fifo->esize = esize; | |
51 | ||
52 | if (size < 2) { | |
53 | fifo->data = NULL; | |
54 | fifo->mask = 0; | |
55 | return -EINVAL; | |
56 | } | |
57 | ||
58 | fifo->data = kmalloc(size * esize, gfp_mask); | |
59 | ||
60 | if (!fifo->data) { | |
61 | fifo->mask = 0; | |
62 | return -ENOMEM; | |
63 | } | |
64 | fifo->mask = size - 1; | |
65 | ||
66 | return 0; | |
67 | } | |
68 | EXPORT_SYMBOL(__kfifo_alloc); | |
69 | ||
70 | void __kfifo_free(struct __kfifo *fifo) | |
71 | { | |
72 | kfree(fifo->data); | |
73 | fifo->in = 0; | |
74 | fifo->out = 0; | |
75 | fifo->esize = 0; | |
76 | fifo->data = NULL; | |
77 | fifo->mask = 0; | |
78 | } | |
79 | EXPORT_SYMBOL(__kfifo_free); | |
80 | ||
81 | int __kfifo_init(struct __kfifo *fifo, void *buffer, | |
82 | unsigned int size, size_t esize) | |
83 | { | |
84 | size /= esize; | |
85 | ||
86 | if (!is_power_of_2(size)) | |
87 | size = rounddown_pow_of_two(size); | |
88 | ||
89 | fifo->in = 0; | |
90 | fifo->out = 0; | |
91 | fifo->esize = esize; | |
92 | fifo->data = buffer; | |
93 | ||
94 | if (size < 2) { | |
95 | fifo->mask = 0; | |
96 | return -EINVAL; | |
97 | } | |
98 | fifo->mask = size - 1; | |
99 | ||
100 | return 0; | |
101 | } | |
102 | EXPORT_SYMBOL(__kfifo_init); | |
103 | ||
104 | static void kfifo_copy_in(struct __kfifo *fifo, const void *src, | |
105 | unsigned int len, unsigned int off) | |
106 | { | |
107 | unsigned int size = fifo->mask + 1; | |
108 | unsigned int esize = fifo->esize; | |
109 | unsigned int l; | |
110 | ||
111 | off &= fifo->mask; | |
112 | if (esize != 1) { | |
113 | off *= esize; | |
114 | size *= esize; | |
115 | len *= esize; | |
116 | } | |
117 | l = min(len, size - off); | |
118 | ||
119 | memcpy(fifo->data + off, src, l); | |
120 | memcpy(fifo->data, src + l, len - l); | |
121 | /* | |
122 | * make sure that the data in the fifo is up to date before | |
123 | * incrementing the fifo->in index counter | |
124 | */ | |
125 | smp_wmb(); | |
126 | } | |
127 | ||
128 | unsigned int __kfifo_in(struct __kfifo *fifo, | |
129 | const void *buf, unsigned int len) | |
130 | { | |
131 | unsigned int l; | |
132 | ||
133 | l = kfifo_unused(fifo); | |
134 | if (len > l) | |
135 | len = l; | |
136 | ||
137 | kfifo_copy_in(fifo, buf, len, fifo->in); | |
138 | fifo->in += len; | |
139 | return len; | |
140 | } | |
141 | EXPORT_SYMBOL(__kfifo_in); | |
142 | ||
143 | static void kfifo_copy_out(struct __kfifo *fifo, void *dst, | |
144 | unsigned int len, unsigned int off) | |
145 | { | |
146 | unsigned int size = fifo->mask + 1; | |
147 | unsigned int esize = fifo->esize; | |
148 | unsigned int l; | |
149 | ||
150 | off &= fifo->mask; | |
151 | if (esize != 1) { | |
152 | off *= esize; | |
153 | size *= esize; | |
154 | len *= esize; | |
155 | } | |
156 | l = min(len, size - off); | |
157 | ||
158 | memcpy(dst, fifo->data + off, l); | |
159 | memcpy(dst + l, fifo->data, len - l); | |
160 | /* | |
161 | * make sure that the data is copied before | |
162 | * incrementing the fifo->out index counter | |
163 | */ | |
164 | smp_wmb(); | |
165 | } | |
166 | ||
167 | unsigned int __kfifo_out_peek(struct __kfifo *fifo, | |
168 | void *buf, unsigned int len) | |
169 | { | |
170 | unsigned int l; | |
171 | ||
172 | l = fifo->in - fifo->out; | |
173 | if (len > l) | |
174 | len = l; | |
175 | ||
176 | kfifo_copy_out(fifo, buf, len, fifo->out); | |
177 | return len; | |
178 | } | |
179 | EXPORT_SYMBOL(__kfifo_out_peek); | |
180 | ||
181 | unsigned int __kfifo_out(struct __kfifo *fifo, | |
182 | void *buf, unsigned int len) | |
183 | { | |
184 | len = __kfifo_out_peek(fifo, buf, len); | |
185 | fifo->out += len; | |
186 | return len; | |
187 | } | |
188 | EXPORT_SYMBOL(__kfifo_out); | |
189 | ||
190 | static unsigned long kfifo_copy_from_user(struct __kfifo *fifo, | |
191 | const void __user *from, unsigned int len, unsigned int off, | |
192 | unsigned int *copied) | |
193 | { | |
194 | unsigned int size = fifo->mask + 1; | |
195 | unsigned int esize = fifo->esize; | |
196 | unsigned int l; | |
197 | unsigned long ret; | |
198 | ||
199 | off &= fifo->mask; | |
200 | if (esize != 1) { | |
201 | off *= esize; | |
202 | size *= esize; | |
203 | len *= esize; | |
204 | } | |
205 | l = min(len, size - off); | |
206 | ||
207 | ret = copy_from_user(fifo->data + off, from, l); | |
208 | if (unlikely(ret)) | |
209 | ret = DIV_ROUND_UP(ret + len - l, esize); | |
210 | else { | |
211 | ret = copy_from_user(fifo->data, from + l, len - l); | |
212 | if (unlikely(ret)) | |
213 | ret = DIV_ROUND_UP(ret, esize); | |
214 | } | |
215 | /* | |
216 | * make sure that the data in the fifo is up to date before | |
217 | * incrementing the fifo->in index counter | |
218 | */ | |
219 | smp_wmb(); | |
220 | *copied = len - ret; | |
221 | /* return the number of elements which are not copied */ | |
222 | return ret; | |
223 | } | |
224 | ||
225 | int __kfifo_from_user(struct __kfifo *fifo, const void __user *from, | |
226 | unsigned long len, unsigned int *copied) | |
227 | { | |
228 | unsigned int l; | |
229 | unsigned long ret; | |
230 | unsigned int esize = fifo->esize; | |
231 | int err; | |
232 | ||
233 | if (esize != 1) | |
234 | len /= esize; | |
235 | ||
236 | l = kfifo_unused(fifo); | |
237 | if (len > l) | |
238 | len = l; | |
239 | ||
240 | ret = kfifo_copy_from_user(fifo, from, len, fifo->in, copied); | |
241 | if (unlikely(ret)) { | |
242 | len -= ret; | |
243 | err = -EFAULT; | |
244 | } else | |
245 | err = 0; | |
246 | fifo->in += len; | |
247 | return err; | |
248 | } | |
249 | EXPORT_SYMBOL(__kfifo_from_user); | |
250 | ||
251 | static unsigned long kfifo_copy_to_user(struct __kfifo *fifo, void __user *to, | |
252 | unsigned int len, unsigned int off, unsigned int *copied) | |
253 | { | |
254 | unsigned int l; | |
255 | unsigned long ret; | |
256 | unsigned int size = fifo->mask + 1; | |
257 | unsigned int esize = fifo->esize; | |
258 | ||
259 | off &= fifo->mask; | |
260 | if (esize != 1) { | |
261 | off *= esize; | |
262 | size *= esize; | |
263 | len *= esize; | |
264 | } | |
265 | l = min(len, size - off); | |
266 | ||
267 | ret = copy_to_user(to, fifo->data + off, l); | |
268 | if (unlikely(ret)) | |
269 | ret = DIV_ROUND_UP(ret + len - l, esize); | |
270 | else { | |
271 | ret = copy_to_user(to + l, fifo->data, len - l); | |
272 | if (unlikely(ret)) | |
273 | ret = DIV_ROUND_UP(ret, esize); | |
274 | } | |
275 | /* | |
276 | * make sure that the data is copied before | |
277 | * incrementing the fifo->out index counter | |
278 | */ | |
279 | smp_wmb(); | |
280 | *copied = len - ret; | |
281 | /* return the number of elements which are not copied */ | |
282 | return ret; | |
283 | } | |
284 | ||
285 | int __kfifo_to_user(struct __kfifo *fifo, void __user *to, | |
286 | unsigned long len, unsigned int *copied) | |
287 | { | |
288 | unsigned int l; | |
289 | unsigned long ret; | |
290 | unsigned int esize = fifo->esize; | |
291 | int err; | |
292 | ||
293 | if (esize != 1) | |
294 | len /= esize; | |
295 | ||
296 | l = fifo->in - fifo->out; | |
297 | if (len > l) | |
298 | len = l; | |
299 | ret = kfifo_copy_to_user(fifo, to, len, fifo->out, copied); | |
300 | if (unlikely(ret)) { | |
301 | len -= ret; | |
302 | err = -EFAULT; | |
303 | } else | |
304 | err = 0; | |
305 | fifo->out += len; | |
306 | return err; | |
307 | } | |
308 | EXPORT_SYMBOL(__kfifo_to_user); | |
309 | ||
310 | static int setup_sgl_buf(struct scatterlist *sgl, void *buf, | |
311 | int nents, unsigned int len) | |
312 | { | |
313 | int n; | |
314 | unsigned int l; | |
315 | unsigned int off; | |
316 | struct page *page; | |
317 | ||
318 | if (!nents) | |
319 | return 0; | |
320 | ||
321 | if (!len) | |
322 | return 0; | |
323 | ||
324 | n = 0; | |
325 | page = virt_to_page(buf); | |
326 | off = offset_in_page(buf); | |
327 | l = 0; | |
328 | ||
329 | while (len >= l + PAGE_SIZE - off) { | |
330 | struct page *npage; | |
331 | ||
332 | l += PAGE_SIZE; | |
333 | buf += PAGE_SIZE; | |
334 | npage = virt_to_page(buf); | |
335 | if (page_to_phys(page) != page_to_phys(npage) - l) { | |
336 | sg_set_page(sgl, page, l - off, off); | |
337 | sgl = sg_next(sgl); | |
338 | if (++n == nents || sgl == NULL) | |
339 | return n; | |
340 | page = npage; | |
341 | len -= l - off; | |
342 | l = off = 0; | |
343 | } | |
344 | } | |
345 | sg_set_page(sgl, page, len, off); | |
346 | return n + 1; | |
347 | } | |
348 | ||
349 | static unsigned int setup_sgl(struct __kfifo *fifo, struct scatterlist *sgl, | |
350 | int nents, unsigned int len, unsigned int off) | |
351 | { | |
352 | unsigned int size = fifo->mask + 1; | |
353 | unsigned int esize = fifo->esize; | |
354 | unsigned int l; | |
355 | unsigned int n; | |
356 | ||
357 | off &= fifo->mask; | |
358 | if (esize != 1) { | |
359 | off *= esize; | |
360 | size *= esize; | |
361 | len *= esize; | |
362 | } | |
363 | l = min(len, size - off); | |
364 | ||
365 | n = setup_sgl_buf(sgl, fifo->data + off, nents, l); | |
366 | n += setup_sgl_buf(sgl + n, fifo->data, nents - n, len - l); | |
367 | ||
368 | return n; | |
369 | } | |
370 | ||
371 | unsigned int __kfifo_dma_in_prepare(struct __kfifo *fifo, | |
372 | struct scatterlist *sgl, int nents, unsigned int len) | |
373 | { | |
374 | unsigned int l; | |
375 | ||
376 | l = kfifo_unused(fifo); | |
377 | if (len > l) | |
378 | len = l; | |
379 | ||
380 | return setup_sgl(fifo, sgl, nents, len, fifo->in); | |
381 | } | |
382 | EXPORT_SYMBOL(__kfifo_dma_in_prepare); | |
383 | ||
384 | unsigned int __kfifo_dma_out_prepare(struct __kfifo *fifo, | |
385 | struct scatterlist *sgl, int nents, unsigned int len) | |
386 | { | |
387 | unsigned int l; | |
388 | ||
389 | l = fifo->in - fifo->out; | |
390 | if (len > l) | |
391 | len = l; | |
392 | ||
393 | return setup_sgl(fifo, sgl, nents, len, fifo->out); | |
394 | } | |
395 | EXPORT_SYMBOL(__kfifo_dma_out_prepare); | |
396 | ||
397 | unsigned int __kfifo_max_r(unsigned int len, size_t recsize) | |
398 | { | |
399 | unsigned int max = (1 << (recsize << 3)) - 1; | |
400 | ||
401 | if (len > max) | |
402 | return max; | |
403 | return len; | |
404 | } | |
405 | ||
406 | #define __KFIFO_PEEK(data, out, mask) \ | |
407 | ((data)[(out) & (mask)]) | |
408 | /* | |
409 | * __kfifo_peek_n internal helper function for determinate the length of | |
410 | * the next record in the fifo | |
411 | */ | |
412 | static unsigned int __kfifo_peek_n(struct __kfifo *fifo, size_t recsize) | |
413 | { | |
414 | unsigned int l; | |
415 | unsigned int mask = fifo->mask; | |
416 | unsigned char *data = fifo->data; | |
417 | ||
418 | l = __KFIFO_PEEK(data, fifo->out, mask); | |
419 | ||
420 | if (--recsize) | |
421 | l |= __KFIFO_PEEK(data, fifo->out + 1, mask) << 8; | |
422 | ||
423 | return l; | |
424 | } | |
425 | ||
426 | #define __KFIFO_POKE(data, in, mask, val) \ | |
427 | ( \ | |
428 | (data)[(in) & (mask)] = (unsigned char)(val) \ | |
429 | ) | |
430 | ||
431 | /* | |
432 | * __kfifo_poke_n internal helper function for storeing the length of | |
433 | * the record into the fifo | |
434 | */ | |
435 | static void __kfifo_poke_n(struct __kfifo *fifo, unsigned int n, size_t recsize) | |
436 | { | |
437 | unsigned int mask = fifo->mask; | |
438 | unsigned char *data = fifo->data; | |
439 | ||
440 | __KFIFO_POKE(data, fifo->in, mask, n); | |
441 | ||
442 | if (recsize > 1) | |
443 | __KFIFO_POKE(data, fifo->in + 1, mask, n >> 8); | |
444 | } | |
445 | ||
446 | unsigned int __kfifo_len_r(struct __kfifo *fifo, size_t recsize) | |
447 | { | |
448 | return __kfifo_peek_n(fifo, recsize); | |
449 | } | |
450 | EXPORT_SYMBOL(__kfifo_len_r); | |
451 | ||
452 | unsigned int __kfifo_in_r(struct __kfifo *fifo, const void *buf, | |
453 | unsigned int len, size_t recsize) | |
454 | { | |
455 | if (len + recsize > kfifo_unused(fifo)) | |
456 | return 0; | |
457 | ||
458 | __kfifo_poke_n(fifo, len, recsize); | |
459 | ||
460 | kfifo_copy_in(fifo, buf, len, fifo->in + recsize); | |
461 | fifo->in += len + recsize; | |
462 | return len; | |
463 | } | |
464 | EXPORT_SYMBOL(__kfifo_in_r); | |
465 | ||
466 | static unsigned int kfifo_out_copy_r(struct __kfifo *fifo, | |
467 | void *buf, unsigned int len, size_t recsize, unsigned int *n) | |
468 | { | |
469 | *n = __kfifo_peek_n(fifo, recsize); | |
470 | ||
471 | if (len > *n) | |
472 | len = *n; | |
473 | ||
474 | kfifo_copy_out(fifo, buf, len, fifo->out + recsize); | |
475 | return len; | |
476 | } | |
477 | ||
478 | unsigned int __kfifo_out_peek_r(struct __kfifo *fifo, void *buf, | |
479 | unsigned int len, size_t recsize) | |
480 | { | |
481 | unsigned int n; | |
482 | ||
483 | if (fifo->in == fifo->out) | |
484 | return 0; | |
485 | ||
486 | return kfifo_out_copy_r(fifo, buf, len, recsize, &n); | |
487 | } | |
488 | EXPORT_SYMBOL(__kfifo_out_peek_r); | |
489 | ||
490 | unsigned int __kfifo_out_r(struct __kfifo *fifo, void *buf, | |
491 | unsigned int len, size_t recsize) | |
492 | { | |
493 | unsigned int n; | |
494 | ||
495 | if (fifo->in == fifo->out) | |
496 | return 0; | |
497 | ||
498 | len = kfifo_out_copy_r(fifo, buf, len, recsize, &n); | |
499 | fifo->out += n + recsize; | |
500 | return len; | |
501 | } | |
502 | EXPORT_SYMBOL(__kfifo_out_r); | |
503 | ||
504 | void __kfifo_skip_r(struct __kfifo *fifo, size_t recsize) | |
505 | { | |
506 | unsigned int n; | |
507 | ||
508 | n = __kfifo_peek_n(fifo, recsize); | |
509 | fifo->out += n + recsize; | |
510 | } | |
511 | EXPORT_SYMBOL(__kfifo_skip_r); | |
512 | ||
513 | int __kfifo_from_user_r(struct __kfifo *fifo, const void __user *from, | |
514 | unsigned long len, unsigned int *copied, size_t recsize) | |
515 | { | |
516 | unsigned long ret; | |
517 | ||
518 | len = __kfifo_max_r(len, recsize); | |
519 | ||
520 | if (len + recsize > kfifo_unused(fifo)) { | |
521 | *copied = 0; | |
522 | return 0; | |
523 | } | |
524 | ||
525 | __kfifo_poke_n(fifo, len, recsize); | |
526 | ||
527 | ret = kfifo_copy_from_user(fifo, from, len, fifo->in + recsize, copied); | |
528 | if (unlikely(ret)) { | |
529 | *copied = 0; | |
530 | return -EFAULT; | |
531 | } | |
532 | fifo->in += len + recsize; | |
533 | return 0; | |
534 | } | |
535 | EXPORT_SYMBOL(__kfifo_from_user_r); | |
536 | ||
537 | int __kfifo_to_user_r(struct __kfifo *fifo, void __user *to, | |
538 | unsigned long len, unsigned int *copied, size_t recsize) | |
539 | { | |
540 | unsigned long ret; | |
541 | unsigned int n; | |
542 | ||
543 | if (fifo->in == fifo->out) { | |
544 | *copied = 0; | |
545 | return 0; | |
546 | } | |
547 | ||
548 | n = __kfifo_peek_n(fifo, recsize); | |
549 | if (len > n) | |
550 | len = n; | |
551 | ||
552 | ret = kfifo_copy_to_user(fifo, to, len, fifo->out + recsize, copied); | |
553 | if (unlikely(ret)) { | |
554 | *copied = 0; | |
555 | return -EFAULT; | |
556 | } | |
557 | fifo->out += n + recsize; | |
558 | return 0; | |
559 | } | |
560 | EXPORT_SYMBOL(__kfifo_to_user_r); | |
561 | ||
562 | unsigned int __kfifo_dma_in_prepare_r(struct __kfifo *fifo, | |
563 | struct scatterlist *sgl, int nents, unsigned int len, size_t recsize) | |
564 | { | |
565 | if (!nents) | |
566 | BUG(); | |
567 | ||
568 | len = __kfifo_max_r(len, recsize); | |
569 | ||
570 | if (len + recsize > kfifo_unused(fifo)) | |
571 | return 0; | |
572 | ||
573 | return setup_sgl(fifo, sgl, nents, len, fifo->in + recsize); | |
574 | } | |
575 | EXPORT_SYMBOL(__kfifo_dma_in_prepare_r); | |
576 | ||
577 | void __kfifo_dma_in_finish_r(struct __kfifo *fifo, | |
578 | unsigned int len, size_t recsize) | |
579 | { | |
580 | len = __kfifo_max_r(len, recsize); | |
581 | __kfifo_poke_n(fifo, len, recsize); | |
582 | fifo->in += len + recsize; | |
583 | } | |
584 | EXPORT_SYMBOL(__kfifo_dma_in_finish_r); | |
585 | ||
586 | unsigned int __kfifo_dma_out_prepare_r(struct __kfifo *fifo, | |
587 | struct scatterlist *sgl, int nents, unsigned int len, size_t recsize) | |
588 | { | |
589 | if (!nents) | |
590 | BUG(); | |
591 | ||
592 | len = __kfifo_max_r(len, recsize); | |
593 | ||
594 | if (len + recsize > fifo->in - fifo->out) | |
595 | return 0; | |
596 | ||
597 | return setup_sgl(fifo, sgl, nents, len, fifo->out + recsize); | |
598 | } | |
599 | EXPORT_SYMBOL(__kfifo_dma_out_prepare_r); | |
600 | ||
601 | void __kfifo_dma_out_finish_r(struct __kfifo *fifo, size_t recsize) | |
602 | { | |
603 | unsigned int len; | |
604 | ||
605 | len = __kfifo_peek_n(fifo, recsize); | |
606 | fifo->out += len + recsize; | |
607 | } | |
608 | EXPORT_SYMBOL(__kfifo_dma_out_finish_r); |