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