diff options
Diffstat (limited to 'kernel')
-rw-r--r-- | kernel/kfifo-new.c | 602 | ||||
-rw-r--r-- | kernel/kfifo.c | 749 |
2 files changed, 453 insertions, 898 deletions
diff --git a/kernel/kfifo-new.c b/kernel/kfifo-new.c deleted file mode 100644 index 02192dd905cc..000000000000 --- a/kernel/kfifo-new.c +++ /dev/null | |||
@@ -1,602 +0,0 @@ | |||
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 | sgl->page_link = 0; | ||
337 | sg_set_page(sgl++, page, l - off, off); | ||
338 | if (++n == nents) | ||
339 | return n; | ||
340 | page = npage; | ||
341 | len -= l - off; | ||
342 | l = off = 0; | ||
343 | } | ||
344 | } | ||
345 | sgl->page_link = 0; | ||
346 | sg_set_page(sgl++, page, len, off); | ||
347 | return n + 1; | ||
348 | } | ||
349 | |||
350 | static unsigned int setup_sgl(struct __kfifo *fifo, struct scatterlist *sgl, | ||
351 | int nents, unsigned int len, unsigned int off) | ||
352 | { | ||
353 | unsigned int size = fifo->mask + 1; | ||
354 | unsigned int esize = fifo->esize; | ||
355 | unsigned int l; | ||
356 | unsigned int n; | ||
357 | |||
358 | off &= fifo->mask; | ||
359 | if (esize != 1) { | ||
360 | off *= esize; | ||
361 | size *= esize; | ||
362 | len *= esize; | ||
363 | } | ||
364 | l = min(len, size - off); | ||
365 | |||
366 | n = setup_sgl_buf(sgl, fifo->data + off, nents, l); | ||
367 | n += setup_sgl_buf(sgl + n, fifo->data, nents - n, len - l); | ||
368 | |||
369 | if (n) | ||
370 | sg_mark_end(sgl + n - 1); | ||
371 | return n; | ||
372 | } | ||
373 | |||
374 | unsigned int __kfifo_dma_in_prepare(struct __kfifo *fifo, | ||
375 | struct scatterlist *sgl, int nents, unsigned int len) | ||
376 | { | ||
377 | unsigned int l; | ||
378 | |||
379 | l = kfifo_unused(fifo); | ||
380 | if (len > l) | ||
381 | len = l; | ||
382 | |||
383 | return setup_sgl(fifo, sgl, nents, len, fifo->in); | ||
384 | } | ||
385 | EXPORT_SYMBOL(__kfifo_dma_in_prepare); | ||
386 | |||
387 | unsigned int __kfifo_dma_out_prepare(struct __kfifo *fifo, | ||
388 | struct scatterlist *sgl, int nents, unsigned int len) | ||
389 | { | ||
390 | unsigned int l; | ||
391 | |||
392 | l = fifo->in - fifo->out; | ||
393 | if (len > l) | ||
394 | len = l; | ||
395 | |||
396 | return setup_sgl(fifo, sgl, nents, len, fifo->out); | ||
397 | } | ||
398 | EXPORT_SYMBOL(__kfifo_dma_out_prepare); | ||
399 | |||
400 | unsigned int __kfifo_max_r(unsigned int len, size_t recsize) | ||
401 | { | ||
402 | unsigned int max = (1 << (recsize << 3)) - 1; | ||
403 | |||
404 | if (len > max) | ||
405 | return max; | ||
406 | return len; | ||
407 | } | ||
408 | |||
409 | #define __KFIFO_PEEK(data, out, mask) \ | ||
410 | ((data)[(out) & (mask)]) | ||
411 | /* | ||
412 | * __kfifo_peek_n internal helper function for determinate the length of | ||
413 | * the next record in the fifo | ||
414 | */ | ||
415 | static unsigned int __kfifo_peek_n(struct __kfifo *fifo, size_t recsize) | ||
416 | { | ||
417 | unsigned int l; | ||
418 | unsigned int mask = fifo->mask; | ||
419 | unsigned char *data = fifo->data; | ||
420 | |||
421 | l = __KFIFO_PEEK(data, fifo->out, mask); | ||
422 | |||
423 | if (--recsize) | ||
424 | l |= __KFIFO_PEEK(data, fifo->out + 1, mask) << 8; | ||
425 | |||
426 | return l; | ||
427 | } | ||
428 | |||
429 | #define __KFIFO_POKE(data, in, mask, val) \ | ||
430 | ( \ | ||
431 | (data)[(in) & (mask)] = (unsigned char)(val) \ | ||
432 | ) | ||
433 | |||
434 | /* | ||
435 | * __kfifo_poke_n internal helper function for storeing the length of | ||
436 | * the record into the fifo | ||
437 | */ | ||
438 | static void __kfifo_poke_n(struct __kfifo *fifo, unsigned int n, size_t recsize) | ||
439 | { | ||
440 | unsigned int mask = fifo->mask; | ||
441 | unsigned char *data = fifo->data; | ||
442 | |||
443 | __KFIFO_POKE(data, fifo->in, mask, n); | ||
444 | |||
445 | if (recsize > 1) | ||
446 | __KFIFO_POKE(data, fifo->in + 1, mask, n >> 8); | ||
447 | } | ||
448 | |||
449 | unsigned int __kfifo_len_r(struct __kfifo *fifo, size_t recsize) | ||
450 | { | ||
451 | return __kfifo_peek_n(fifo, recsize); | ||
452 | } | ||
453 | EXPORT_SYMBOL(__kfifo_len_r); | ||
454 | |||
455 | unsigned int __kfifo_in_r(struct __kfifo *fifo, const void *buf, | ||
456 | unsigned int len, size_t recsize) | ||
457 | { | ||
458 | if (len + recsize > kfifo_unused(fifo)) | ||
459 | return 0; | ||
460 | |||
461 | __kfifo_poke_n(fifo, len, recsize); | ||
462 | |||
463 | kfifo_copy_in(fifo, buf, len, fifo->in + recsize); | ||
464 | fifo->in += len + recsize; | ||
465 | return len; | ||
466 | } | ||
467 | EXPORT_SYMBOL(__kfifo_in_r); | ||
468 | |||
469 | static unsigned int kfifo_out_copy_r(struct __kfifo *fifo, | ||
470 | void *buf, unsigned int len, size_t recsize, unsigned int *n) | ||
471 | { | ||
472 | *n = __kfifo_peek_n(fifo, recsize); | ||
473 | |||
474 | if (len > *n) | ||
475 | len = *n; | ||
476 | |||
477 | kfifo_copy_out(fifo, buf, len, fifo->out + recsize); | ||
478 | return len; | ||
479 | } | ||
480 | |||
481 | unsigned int __kfifo_out_peek_r(struct __kfifo *fifo, void *buf, | ||
482 | unsigned int len, size_t recsize) | ||
483 | { | ||
484 | unsigned int n; | ||
485 | |||
486 | if (fifo->in == fifo->out) | ||
487 | return 0; | ||
488 | |||
489 | return kfifo_out_copy_r(fifo, buf, len, recsize, &n); | ||
490 | } | ||
491 | EXPORT_SYMBOL(__kfifo_out_peek_r); | ||
492 | |||
493 | unsigned int __kfifo_out_r(struct __kfifo *fifo, void *buf, | ||
494 | unsigned int len, size_t recsize) | ||
495 | { | ||
496 | unsigned int n; | ||
497 | |||
498 | if (fifo->in == fifo->out) | ||
499 | return 0; | ||
500 | |||
501 | len = kfifo_out_copy_r(fifo, buf, len, recsize, &n); | ||
502 | fifo->out += n + recsize; | ||
503 | return len; | ||
504 | } | ||
505 | EXPORT_SYMBOL(__kfifo_out_r); | ||
506 | |||
507 | int __kfifo_from_user_r(struct __kfifo *fifo, const void __user *from, | ||
508 | unsigned long len, unsigned int *copied, size_t recsize) | ||
509 | { | ||
510 | unsigned long ret; | ||
511 | |||
512 | len = __kfifo_max_r(len, recsize); | ||
513 | |||
514 | if (len + recsize > kfifo_unused(fifo)) { | ||
515 | *copied = 0; | ||
516 | return 0; | ||
517 | } | ||
518 | |||
519 | __kfifo_poke_n(fifo, len, recsize); | ||
520 | |||
521 | ret = kfifo_copy_from_user(fifo, from, len, fifo->in + recsize, copied); | ||
522 | if (unlikely(ret)) { | ||
523 | *copied = 0; | ||
524 | return -EFAULT; | ||
525 | } | ||
526 | fifo->in += len + recsize; | ||
527 | return 0; | ||
528 | } | ||
529 | EXPORT_SYMBOL(__kfifo_from_user_r); | ||
530 | |||
531 | int __kfifo_to_user_r(struct __kfifo *fifo, void __user *to, | ||
532 | unsigned long len, unsigned int *copied, size_t recsize) | ||
533 | { | ||
534 | unsigned long ret; | ||
535 | unsigned int n; | ||
536 | |||
537 | if (fifo->in == fifo->out) { | ||
538 | *copied = 0; | ||
539 | return 0; | ||
540 | } | ||
541 | |||
542 | n = __kfifo_peek_n(fifo, recsize); | ||
543 | if (len > n) | ||
544 | len = n; | ||
545 | |||
546 | ret = kfifo_copy_to_user(fifo, to, len, fifo->out + recsize, copied); | ||
547 | if (unlikely(ret)) { | ||
548 | *copied = 0; | ||
549 | return -EFAULT; | ||
550 | } | ||
551 | fifo->out += n + recsize; | ||
552 | return 0; | ||
553 | } | ||
554 | EXPORT_SYMBOL(__kfifo_to_user_r); | ||
555 | |||
556 | unsigned int __kfifo_dma_in_prepare_r(struct __kfifo *fifo, | ||
557 | struct scatterlist *sgl, int nents, unsigned int len, size_t recsize) | ||
558 | { | ||
559 | if (!nents) | ||
560 | BUG(); | ||
561 | |||
562 | len = __kfifo_max_r(len, recsize); | ||
563 | |||
564 | if (len + recsize > kfifo_unused(fifo)) | ||
565 | return 0; | ||
566 | |||
567 | return setup_sgl(fifo, sgl, nents, len, fifo->in + recsize); | ||
568 | } | ||
569 | EXPORT_SYMBOL(__kfifo_dma_in_prepare_r); | ||
570 | |||
571 | void __kfifo_dma_in_finish_r(struct __kfifo *fifo, | ||
572 | unsigned int len, size_t recsize) | ||
573 | { | ||
574 | len = __kfifo_max_r(len, recsize); | ||
575 | __kfifo_poke_n(fifo, len, recsize); | ||
576 | fifo->in += len + recsize; | ||
577 | } | ||
578 | EXPORT_SYMBOL(__kfifo_dma_in_finish_r); | ||
579 | |||
580 | unsigned int __kfifo_dma_out_prepare_r(struct __kfifo *fifo, | ||
581 | struct scatterlist *sgl, int nents, unsigned int len, size_t recsize) | ||
582 | { | ||
583 | if (!nents) | ||
584 | BUG(); | ||
585 | |||
586 | len = __kfifo_max_r(len, recsize); | ||
587 | |||
588 | if (len + recsize > fifo->in - fifo->out) | ||
589 | return 0; | ||
590 | |||
591 | return setup_sgl(fifo, sgl, nents, len, fifo->out + recsize); | ||
592 | } | ||
593 | EXPORT_SYMBOL(__kfifo_dma_out_prepare_r); | ||
594 | |||
595 | void __kfifo_dma_out_finish_r(struct __kfifo *fifo, size_t recsize) | ||
596 | { | ||
597 | unsigned int len; | ||
598 | |||
599 | len = __kfifo_peek_n(fifo, recsize); | ||
600 | fifo->out += len + recsize; | ||
601 | } | ||
602 | EXPORT_SYMBOL(__kfifo_dma_out_finish_r); | ||
diff --git a/kernel/kfifo.c b/kernel/kfifo.c index 35edbe22e9a9..02192dd905cc 100644 --- a/kernel/kfifo.c +++ b/kernel/kfifo.c | |||
@@ -1,8 +1,7 @@ | |||
1 | /* | 1 | /* |
2 | * A generic kernel FIFO implementation. | 2 | * A generic kernel FIFO implementation |
3 | * | 3 | * |
4 | * Copyright (C) 2009 Stefani Seibold <stefani@seibold.net> | 4 | * Copyright (C) 2009/2010 Stefani Seibold <stefani@seibold.net> |
5 | * Copyright (C) 2004 Stelian Pop <stelian@popies.net> | ||
6 | * | 5 | * |
7 | * This program is free software; you can redistribute it and/or modify | 6 | * This program is free software; you can redistribute it and/or modify |
8 | * it under the terms of the GNU General Public License as published by | 7 | * it under the terms of the GNU General Public License as published by |
@@ -24,422 +23,580 @@ | |||
24 | #include <linux/module.h> | 23 | #include <linux/module.h> |
25 | #include <linux/slab.h> | 24 | #include <linux/slab.h> |
26 | #include <linux/err.h> | 25 | #include <linux/err.h> |
27 | #include <linux/kfifo.h> | ||
28 | #include <linux/log2.h> | 26 | #include <linux/log2.h> |
29 | #include <linux/uaccess.h> | 27 | #include <linux/uaccess.h> |
28 | #include <linux/kfifo.h> | ||
30 | 29 | ||
31 | static void _kfifo_init(struct kfifo *fifo, void *buffer, | 30 | /* |
32 | unsigned int size) | 31 | * internal helper to calculate the unused elements in a fifo |
33 | { | ||
34 | fifo->buffer = buffer; | ||
35 | fifo->size = size; | ||
36 | |||
37 | kfifo_reset(fifo); | ||
38 | } | ||
39 | |||
40 | /** | ||
41 | * kfifo_init - initialize a FIFO using a preallocated buffer | ||
42 | * @fifo: the fifo to assign the buffer | ||
43 | * @buffer: the preallocated buffer to be used. | ||
44 | * @size: the size of the internal buffer, this has to be a power of 2. | ||
45 | * | ||
46 | */ | 32 | */ |
47 | void kfifo_init(struct kfifo *fifo, void *buffer, unsigned int size) | 33 | static inline unsigned int kfifo_unused(struct __kfifo *fifo) |
48 | { | 34 | { |
49 | /* size must be a power of 2 */ | 35 | return (fifo->mask + 1) - (fifo->in - fifo->out); |
50 | BUG_ON(!is_power_of_2(size)); | ||
51 | |||
52 | _kfifo_init(fifo, buffer, size); | ||
53 | } | 36 | } |
54 | EXPORT_SYMBOL(kfifo_init); | ||
55 | 37 | ||
56 | /** | 38 | int __kfifo_alloc(struct __kfifo *fifo, unsigned int size, |
57 | * kfifo_alloc - allocates a new FIFO internal buffer | 39 | size_t esize, gfp_t gfp_mask) |
58 | * @fifo: the fifo to assign then new buffer | ||
59 | * @size: the size of the buffer to be allocated, this have to be a power of 2. | ||
60 | * @gfp_mask: get_free_pages mask, passed to kmalloc() | ||
61 | * | ||
62 | * This function dynamically allocates a new fifo internal buffer | ||
63 | * | ||
64 | * The size will be rounded-up to a power of 2. | ||
65 | * The buffer will be release with kfifo_free(). | ||
66 | * Return 0 if no error, otherwise the an error code | ||
67 | */ | ||
68 | int kfifo_alloc(struct kfifo *fifo, unsigned int size, gfp_t gfp_mask) | ||
69 | { | 40 | { |
70 | unsigned char *buffer; | ||
71 | |||
72 | /* | 41 | /* |
73 | * round up to the next power of 2, since our 'let the indices | 42 | * round down to the next power of 2, since our 'let the indices |
74 | * wrap' technique works only in this case. | 43 | * wrap' technique works only in this case. |
75 | */ | 44 | */ |
76 | if (!is_power_of_2(size)) { | 45 | if (!is_power_of_2(size)) |
77 | BUG_ON(size > 0x80000000); | 46 | size = rounddown_pow_of_two(size); |
78 | size = roundup_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; | ||
79 | } | 56 | } |
80 | 57 | ||
81 | buffer = kmalloc(size, gfp_mask); | 58 | fifo->data = kmalloc(size * esize, gfp_mask); |
82 | if (!buffer) { | 59 | |
83 | _kfifo_init(fifo, NULL, 0); | 60 | if (!fifo->data) { |
61 | fifo->mask = 0; | ||
84 | return -ENOMEM; | 62 | return -ENOMEM; |
85 | } | 63 | } |
86 | 64 | fifo->mask = size - 1; | |
87 | _kfifo_init(fifo, buffer, size); | ||
88 | 65 | ||
89 | return 0; | 66 | return 0; |
90 | } | 67 | } |
91 | EXPORT_SYMBOL(kfifo_alloc); | 68 | EXPORT_SYMBOL(__kfifo_alloc); |
92 | 69 | ||
93 | /** | 70 | void __kfifo_free(struct __kfifo *fifo) |
94 | * kfifo_free - frees the FIFO internal buffer | ||
95 | * @fifo: the fifo to be freed. | ||
96 | */ | ||
97 | void kfifo_free(struct kfifo *fifo) | ||
98 | { | 71 | { |
99 | kfree(fifo->buffer); | 72 | kfree(fifo->data); |
100 | _kfifo_init(fifo, NULL, 0); | 73 | fifo->in = 0; |
74 | fifo->out = 0; | ||
75 | fifo->esize = 0; | ||
76 | fifo->data = NULL; | ||
77 | fifo->mask = 0; | ||
101 | } | 78 | } |
102 | EXPORT_SYMBOL(kfifo_free); | 79 | EXPORT_SYMBOL(__kfifo_free); |
103 | 80 | ||
104 | /** | 81 | int __kfifo_init(struct __kfifo *fifo, void *buffer, |
105 | * kfifo_skip - skip output data | 82 | unsigned int size, size_t esize) |
106 | * @fifo: the fifo to be used. | ||
107 | * @len: number of bytes to skip | ||
108 | */ | ||
109 | void kfifo_skip(struct kfifo *fifo, unsigned int len) | ||
110 | { | 83 | { |
111 | if (len < kfifo_len(fifo)) { | 84 | size /= esize; |
112 | __kfifo_add_out(fifo, len); | 85 | |
113 | return; | 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; | ||
114 | } | 97 | } |
115 | kfifo_reset_out(fifo); | 98 | fifo->mask = size - 1; |
99 | |||
100 | return 0; | ||
116 | } | 101 | } |
117 | EXPORT_SYMBOL(kfifo_skip); | 102 | EXPORT_SYMBOL(__kfifo_init); |
118 | 103 | ||
119 | static inline void __kfifo_in_data(struct kfifo *fifo, | 104 | static void kfifo_copy_in(struct __kfifo *fifo, const void *src, |
120 | const void *from, unsigned int len, unsigned int off) | 105 | unsigned int len, unsigned int off) |
121 | { | 106 | { |
107 | unsigned int size = fifo->mask + 1; | ||
108 | unsigned int esize = fifo->esize; | ||
122 | unsigned int l; | 109 | unsigned int l; |
123 | 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); | ||
124 | /* | 121 | /* |
125 | * Ensure that we sample the fifo->out index -before- we | 122 | * make sure that the data in the fifo is up to date before |
126 | * start putting bytes into the kfifo. | 123 | * incrementing the fifo->in index counter |
127 | */ | 124 | */ |
125 | smp_wmb(); | ||
126 | } | ||
128 | 127 | ||
129 | smp_mb(); | 128 | unsigned int __kfifo_in(struct __kfifo *fifo, |
130 | 129 | const void *buf, unsigned int len) | |
131 | off = __kfifo_off(fifo, fifo->in + off); | 130 | { |
131 | unsigned int l; | ||
132 | 132 | ||
133 | /* first put the data starting from fifo->in to buffer end */ | 133 | l = kfifo_unused(fifo); |
134 | l = min(len, fifo->size - off); | 134 | if (len > l) |
135 | memcpy(fifo->buffer + off, from, l); | 135 | len = l; |
136 | 136 | ||
137 | /* then put the rest (if any) at the beginning of the buffer */ | 137 | kfifo_copy_in(fifo, buf, len, fifo->in); |
138 | memcpy(fifo->buffer, from + l, len - l); | 138 | fifo->in += len; |
139 | return len; | ||
139 | } | 140 | } |
141 | EXPORT_SYMBOL(__kfifo_in); | ||
140 | 142 | ||
141 | static inline void __kfifo_out_data(struct kfifo *fifo, | 143 | static void kfifo_copy_out(struct __kfifo *fifo, void *dst, |
142 | void *to, unsigned int len, unsigned int off) | 144 | unsigned int len, unsigned int off) |
143 | { | 145 | { |
146 | unsigned int size = fifo->mask + 1; | ||
147 | unsigned int esize = fifo->esize; | ||
144 | unsigned int l; | 148 | unsigned int l; |
145 | 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); | ||
146 | /* | 160 | /* |
147 | * Ensure that we sample the fifo->in index -before- we | 161 | * make sure that the data is copied before |
148 | * start removing bytes from the kfifo. | 162 | * incrementing the fifo->out index counter |
149 | */ | 163 | */ |
164 | smp_wmb(); | ||
165 | } | ||
150 | 166 | ||
151 | smp_rmb(); | 167 | unsigned int __kfifo_out_peek(struct __kfifo *fifo, |
168 | void *buf, unsigned int len) | ||
169 | { | ||
170 | unsigned int l; | ||
152 | 171 | ||
153 | off = __kfifo_off(fifo, fifo->out + off); | 172 | l = fifo->in - fifo->out; |
173 | if (len > l) | ||
174 | len = l; | ||
154 | 175 | ||
155 | /* first get the data from fifo->out until the end of the buffer */ | 176 | kfifo_copy_out(fifo, buf, len, fifo->out); |
156 | l = min(len, fifo->size - off); | 177 | return len; |
157 | memcpy(to, fifo->buffer + off, l); | 178 | } |
179 | EXPORT_SYMBOL(__kfifo_out_peek); | ||
158 | 180 | ||
159 | /* then get the rest (if any) from the beginning of the buffer */ | 181 | unsigned int __kfifo_out(struct __kfifo *fifo, |
160 | memcpy(to + l, fifo->buffer, len - l); | 182 | void *buf, unsigned int len) |
183 | { | ||
184 | len = __kfifo_out_peek(fifo, buf, len); | ||
185 | fifo->out += len; | ||
186 | return len; | ||
161 | } | 187 | } |
188 | EXPORT_SYMBOL(__kfifo_out); | ||
162 | 189 | ||
163 | static inline int __kfifo_from_user_data(struct kfifo *fifo, | 190 | static unsigned long kfifo_copy_from_user(struct __kfifo *fifo, |
164 | const void __user *from, unsigned int len, unsigned int off, | 191 | const void __user *from, unsigned int len, unsigned int off, |
165 | unsigned *lenout) | 192 | unsigned int *copied) |
166 | { | 193 | { |
194 | unsigned int size = fifo->mask + 1; | ||
195 | unsigned int esize = fifo->esize; | ||
167 | unsigned int l; | 196 | unsigned int l; |
168 | int ret; | 197 | unsigned long ret; |
169 | 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 | } | ||
170 | /* | 215 | /* |
171 | * Ensure that we sample the fifo->out index -before- we | 216 | * make sure that the data in the fifo is up to date before |
172 | * start putting bytes into the kfifo. | 217 | * incrementing the fifo->in index counter |
173 | */ | 218 | */ |
219 | smp_wmb(); | ||
220 | *copied = len - ret; | ||
221 | /* return the number of elements which are not copied */ | ||
222 | return ret; | ||
223 | } | ||
174 | 224 | ||
175 | smp_mb(); | 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; | ||
176 | 232 | ||
177 | off = __kfifo_off(fifo, fifo->in + off); | 233 | if (esize != 1) |
234 | len /= esize; | ||
178 | 235 | ||
179 | /* first put the data starting from fifo->in to buffer end */ | 236 | l = kfifo_unused(fifo); |
180 | l = min(len, fifo->size - off); | 237 | if (len > l) |
181 | ret = copy_from_user(fifo->buffer + off, from, l); | 238 | len = l; |
182 | if (unlikely(ret)) { | ||
183 | *lenout = ret; | ||
184 | return -EFAULT; | ||
185 | } | ||
186 | *lenout = l; | ||
187 | 239 | ||
188 | /* then put the rest (if any) at the beginning of the buffer */ | 240 | ret = kfifo_copy_from_user(fifo, from, len, fifo->in, copied); |
189 | ret = copy_from_user(fifo->buffer, from + l, len - l); | 241 | if (unlikely(ret)) { |
190 | *lenout += ret ? ret : len - l; | 242 | len -= ret; |
191 | return ret ? -EFAULT : 0; | 243 | err = -EFAULT; |
244 | } else | ||
245 | err = 0; | ||
246 | fifo->in += len; | ||
247 | return err; | ||
192 | } | 248 | } |
249 | EXPORT_SYMBOL(__kfifo_from_user); | ||
193 | 250 | ||
194 | static inline int __kfifo_to_user_data(struct kfifo *fifo, | 251 | static unsigned long kfifo_copy_to_user(struct __kfifo *fifo, void __user *to, |
195 | void __user *to, unsigned int len, unsigned int off, unsigned *lenout) | 252 | unsigned int len, unsigned int off, unsigned int *copied) |
196 | { | 253 | { |
197 | unsigned int l; | 254 | unsigned int l; |
198 | int ret; | 255 | unsigned long ret; |
199 | 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 | } | ||
200 | /* | 275 | /* |
201 | * Ensure that we sample the fifo->in index -before- we | 276 | * make sure that the data is copied before |
202 | * start removing bytes from the kfifo. | 277 | * incrementing the fifo->out index counter |
203 | */ | 278 | */ |
279 | smp_wmb(); | ||
280 | *copied = len - ret; | ||
281 | /* return the number of elements which are not copied */ | ||
282 | return ret; | ||
283 | } | ||
204 | 284 | ||
205 | smp_rmb(); | 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; | ||
206 | 292 | ||
207 | off = __kfifo_off(fifo, fifo->out + off); | 293 | if (esize != 1) |
294 | len /= esize; | ||
208 | 295 | ||
209 | /* first get the data from fifo->out until the end of the buffer */ | 296 | l = fifo->in - fifo->out; |
210 | l = min(len, fifo->size - off); | 297 | if (len > l) |
211 | ret = copy_to_user(to, fifo->buffer + off, l); | 298 | len = l; |
212 | *lenout = l; | 299 | ret = kfifo_copy_to_user(fifo, to, len, fifo->out, copied); |
213 | if (unlikely(ret)) { | 300 | if (unlikely(ret)) { |
214 | *lenout -= ret; | 301 | len -= ret; |
215 | return -EFAULT; | 302 | err = -EFAULT; |
216 | } | 303 | } else |
304 | err = 0; | ||
305 | fifo->out += len; | ||
306 | return err; | ||
307 | } | ||
308 | EXPORT_SYMBOL(__kfifo_to_user); | ||
217 | 309 | ||
218 | /* then get the rest (if any) from the beginning of the buffer */ | 310 | static int setup_sgl_buf(struct scatterlist *sgl, void *buf, |
219 | len -= l; | 311 | int nents, unsigned int len) |
220 | ret = copy_to_user(to + l, fifo->buffer, len); | 312 | { |
221 | if (unlikely(ret)) { | 313 | int n; |
222 | *lenout += len - ret; | 314 | unsigned int l; |
223 | return -EFAULT; | 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 | sgl->page_link = 0; | ||
337 | sg_set_page(sgl++, page, l - off, off); | ||
338 | if (++n == nents) | ||
339 | return n; | ||
340 | page = npage; | ||
341 | len -= l - off; | ||
342 | l = off = 0; | ||
343 | } | ||
224 | } | 344 | } |
225 | *lenout += len; | 345 | sgl->page_link = 0; |
226 | return 0; | 346 | sg_set_page(sgl++, page, len, off); |
347 | return n + 1; | ||
227 | } | 348 | } |
228 | 349 | ||
229 | unsigned int __kfifo_in_n(struct kfifo *fifo, | 350 | static unsigned int setup_sgl(struct __kfifo *fifo, struct scatterlist *sgl, |
230 | const void *from, unsigned int len, unsigned int recsize) | 351 | int nents, unsigned int len, unsigned int off) |
231 | { | 352 | { |
232 | if (kfifo_avail(fifo) < len + recsize) | 353 | unsigned int size = fifo->mask + 1; |
233 | return len + 1; | 354 | unsigned int esize = fifo->esize; |
355 | unsigned int l; | ||
356 | unsigned int n; | ||
234 | 357 | ||
235 | __kfifo_in_data(fifo, from, len, recsize); | 358 | off &= fifo->mask; |
236 | return 0; | 359 | if (esize != 1) { |
360 | off *= esize; | ||
361 | size *= esize; | ||
362 | len *= esize; | ||
363 | } | ||
364 | l = min(len, size - off); | ||
365 | |||
366 | n = setup_sgl_buf(sgl, fifo->data + off, nents, l); | ||
367 | n += setup_sgl_buf(sgl + n, fifo->data, nents - n, len - l); | ||
368 | |||
369 | if (n) | ||
370 | sg_mark_end(sgl + n - 1); | ||
371 | return n; | ||
237 | } | 372 | } |
238 | EXPORT_SYMBOL(__kfifo_in_n); | ||
239 | 373 | ||
240 | /** | 374 | unsigned int __kfifo_dma_in_prepare(struct __kfifo *fifo, |
241 | * kfifo_in - puts some data into the FIFO | 375 | struct scatterlist *sgl, int nents, unsigned int len) |
242 | * @fifo: the fifo to be used. | ||
243 | * @from: the data to be added. | ||
244 | * @len: the length of the data to be added. | ||
245 | * | ||
246 | * This function copies at most @len bytes from the @from buffer into | ||
247 | * the FIFO depending on the free space, and returns the number of | ||
248 | * bytes copied. | ||
249 | * | ||
250 | * Note that with only one concurrent reader and one concurrent | ||
251 | * writer, you don't need extra locking to use these functions. | ||
252 | */ | ||
253 | unsigned int kfifo_in(struct kfifo *fifo, const void *from, | ||
254 | unsigned int len) | ||
255 | { | 376 | { |
256 | len = min(kfifo_avail(fifo), len); | 377 | unsigned int l; |
257 | 378 | ||
258 | __kfifo_in_data(fifo, from, len, 0); | 379 | l = kfifo_unused(fifo); |
259 | __kfifo_add_in(fifo, len); | 380 | if (len > l) |
260 | return len; | 381 | len = l; |
382 | |||
383 | return setup_sgl(fifo, sgl, nents, len, fifo->in); | ||
261 | } | 384 | } |
262 | EXPORT_SYMBOL(kfifo_in); | 385 | EXPORT_SYMBOL(__kfifo_dma_in_prepare); |
263 | 386 | ||
264 | unsigned int __kfifo_in_generic(struct kfifo *fifo, | 387 | unsigned int __kfifo_dma_out_prepare(struct __kfifo *fifo, |
265 | const void *from, unsigned int len, unsigned int recsize) | 388 | struct scatterlist *sgl, int nents, unsigned int len) |
266 | { | 389 | { |
267 | return __kfifo_in_rec(fifo, from, len, recsize); | 390 | unsigned int l; |
391 | |||
392 | l = fifo->in - fifo->out; | ||
393 | if (len > l) | ||
394 | len = l; | ||
395 | |||
396 | return setup_sgl(fifo, sgl, nents, len, fifo->out); | ||
268 | } | 397 | } |
269 | EXPORT_SYMBOL(__kfifo_in_generic); | 398 | EXPORT_SYMBOL(__kfifo_dma_out_prepare); |
270 | 399 | ||
271 | unsigned int __kfifo_out_n(struct kfifo *fifo, | 400 | unsigned int __kfifo_max_r(unsigned int len, size_t recsize) |
272 | void *to, unsigned int len, unsigned int recsize) | ||
273 | { | 401 | { |
274 | if (kfifo_len(fifo) < len + recsize) | 402 | unsigned int max = (1 << (recsize << 3)) - 1; |
275 | return len; | ||
276 | 403 | ||
277 | __kfifo_out_data(fifo, to, len, recsize); | 404 | if (len > max) |
278 | __kfifo_add_out(fifo, len + recsize); | 405 | return max; |
279 | return 0; | 406 | return len; |
280 | } | 407 | } |
281 | EXPORT_SYMBOL(__kfifo_out_n); | ||
282 | 408 | ||
283 | /** | 409 | #define __KFIFO_PEEK(data, out, mask) \ |
284 | * kfifo_out - gets some data from the FIFO | 410 | ((data)[(out) & (mask)]) |
285 | * @fifo: the fifo to be used. | 411 | /* |
286 | * @to: where the data must be copied. | 412 | * __kfifo_peek_n internal helper function for determinate the length of |
287 | * @len: the size of the destination buffer. | 413 | * the next record in the fifo |
288 | * | ||
289 | * This function copies at most @len bytes from the FIFO into the | ||
290 | * @to buffer and returns the number of copied bytes. | ||
291 | * | ||
292 | * Note that with only one concurrent reader and one concurrent | ||
293 | * writer, you don't need extra locking to use these functions. | ||
294 | */ | 414 | */ |
295 | unsigned int kfifo_out(struct kfifo *fifo, void *to, unsigned int len) | 415 | static unsigned int __kfifo_peek_n(struct __kfifo *fifo, size_t recsize) |
296 | { | 416 | { |
297 | len = min(kfifo_len(fifo), len); | 417 | unsigned int l; |
418 | unsigned int mask = fifo->mask; | ||
419 | unsigned char *data = fifo->data; | ||
298 | 420 | ||
299 | __kfifo_out_data(fifo, to, len, 0); | 421 | l = __KFIFO_PEEK(data, fifo->out, mask); |
300 | __kfifo_add_out(fifo, len); | ||
301 | 422 | ||
302 | return len; | 423 | if (--recsize) |
424 | l |= __KFIFO_PEEK(data, fifo->out + 1, mask) << 8; | ||
425 | |||
426 | return l; | ||
303 | } | 427 | } |
304 | EXPORT_SYMBOL(kfifo_out); | 428 | |
305 | 429 | #define __KFIFO_POKE(data, in, mask, val) \ | |
306 | /** | 430 | ( \ |
307 | * kfifo_out_peek - copy some data from the FIFO, but do not remove it | 431 | (data)[(in) & (mask)] = (unsigned char)(val) \ |
308 | * @fifo: the fifo to be used. | 432 | ) |
309 | * @to: where the data must be copied. | 433 | |
310 | * @len: the size of the destination buffer. | 434 | /* |
311 | * @offset: offset into the fifo | 435 | * __kfifo_poke_n internal helper function for storeing the length of |
312 | * | 436 | * the record into the fifo |
313 | * This function copies at most @len bytes at @offset from the FIFO | ||
314 | * into the @to buffer and returns the number of copied bytes. | ||
315 | * The data is not removed from the FIFO. | ||
316 | */ | 437 | */ |
317 | unsigned int kfifo_out_peek(struct kfifo *fifo, void *to, unsigned int len, | 438 | static void __kfifo_poke_n(struct __kfifo *fifo, unsigned int n, size_t recsize) |
318 | unsigned offset) | ||
319 | { | 439 | { |
320 | len = min(kfifo_len(fifo), len + offset); | 440 | unsigned int mask = fifo->mask; |
441 | unsigned char *data = fifo->data; | ||
321 | 442 | ||
322 | __kfifo_out_data(fifo, to, len, offset); | 443 | __KFIFO_POKE(data, fifo->in, mask, n); |
323 | return len; | 444 | |
445 | if (recsize > 1) | ||
446 | __KFIFO_POKE(data, fifo->in + 1, mask, n >> 8); | ||
324 | } | 447 | } |
325 | EXPORT_SYMBOL(kfifo_out_peek); | ||
326 | 448 | ||
327 | unsigned int __kfifo_out_generic(struct kfifo *fifo, | 449 | unsigned int __kfifo_len_r(struct __kfifo *fifo, size_t recsize) |
328 | void *to, unsigned int len, unsigned int recsize, | ||
329 | unsigned int *total) | ||
330 | { | 450 | { |
331 | return __kfifo_out_rec(fifo, to, len, recsize, total); | 451 | return __kfifo_peek_n(fifo, recsize); |
332 | } | 452 | } |
333 | EXPORT_SYMBOL(__kfifo_out_generic); | 453 | EXPORT_SYMBOL(__kfifo_len_r); |
334 | 454 | ||
335 | unsigned int __kfifo_from_user_n(struct kfifo *fifo, | 455 | unsigned int __kfifo_in_r(struct __kfifo *fifo, const void *buf, |
336 | const void __user *from, unsigned int len, unsigned int recsize) | 456 | unsigned int len, size_t recsize) |
337 | { | 457 | { |
338 | unsigned total; | 458 | if (len + recsize > kfifo_unused(fifo)) |
459 | return 0; | ||
339 | 460 | ||
340 | if (kfifo_avail(fifo) < len + recsize) | 461 | __kfifo_poke_n(fifo, len, recsize); |
341 | return len + 1; | ||
342 | 462 | ||
343 | __kfifo_from_user_data(fifo, from, len, recsize, &total); | 463 | kfifo_copy_in(fifo, buf, len, fifo->in + recsize); |
344 | return total; | 464 | fifo->in += len + recsize; |
465 | return len; | ||
345 | } | 466 | } |
346 | EXPORT_SYMBOL(__kfifo_from_user_n); | 467 | EXPORT_SYMBOL(__kfifo_in_r); |
347 | 468 | ||
348 | /** | 469 | static unsigned int kfifo_out_copy_r(struct __kfifo *fifo, |
349 | * kfifo_from_user - puts some data from user space into the FIFO | 470 | void *buf, unsigned int len, size_t recsize, unsigned int *n) |
350 | * @fifo: the fifo to be used. | ||
351 | * @from: pointer to the data to be added. | ||
352 | * @len: the length of the data to be added. | ||
353 | * @total: the actual returned data length. | ||
354 | * | ||
355 | * This function copies at most @len bytes from the @from into the | ||
356 | * FIFO depending and returns -EFAULT/0. | ||
357 | * | ||
358 | * Note that with only one concurrent reader and one concurrent | ||
359 | * writer, you don't need extra locking to use these functions. | ||
360 | */ | ||
361 | int kfifo_from_user(struct kfifo *fifo, | ||
362 | const void __user *from, unsigned int len, unsigned *total) | ||
363 | { | 471 | { |
364 | int ret; | 472 | *n = __kfifo_peek_n(fifo, recsize); |
365 | len = min(kfifo_avail(fifo), len); | 473 | |
366 | ret = __kfifo_from_user_data(fifo, from, len, 0, total); | 474 | if (len > *n) |
367 | if (ret) | 475 | len = *n; |
368 | return ret; | 476 | |
369 | __kfifo_add_in(fifo, len); | 477 | kfifo_copy_out(fifo, buf, len, fifo->out + recsize); |
370 | return 0; | 478 | return len; |
479 | } | ||
480 | |||
481 | unsigned int __kfifo_out_peek_r(struct __kfifo *fifo, void *buf, | ||
482 | unsigned int len, size_t recsize) | ||
483 | { | ||
484 | unsigned int n; | ||
485 | |||
486 | if (fifo->in == fifo->out) | ||
487 | return 0; | ||
488 | |||
489 | return kfifo_out_copy_r(fifo, buf, len, recsize, &n); | ||
371 | } | 490 | } |
372 | EXPORT_SYMBOL(kfifo_from_user); | 491 | EXPORT_SYMBOL(__kfifo_out_peek_r); |
373 | 492 | ||
374 | unsigned int __kfifo_from_user_generic(struct kfifo *fifo, | 493 | unsigned int __kfifo_out_r(struct __kfifo *fifo, void *buf, |
375 | const void __user *from, unsigned int len, unsigned int recsize) | 494 | unsigned int len, size_t recsize) |
376 | { | 495 | { |
377 | return __kfifo_from_user_rec(fifo, from, len, recsize); | 496 | unsigned int n; |
497 | |||
498 | if (fifo->in == fifo->out) | ||
499 | return 0; | ||
500 | |||
501 | len = kfifo_out_copy_r(fifo, buf, len, recsize, &n); | ||
502 | fifo->out += n + recsize; | ||
503 | return len; | ||
378 | } | 504 | } |
379 | EXPORT_SYMBOL(__kfifo_from_user_generic); | 505 | EXPORT_SYMBOL(__kfifo_out_r); |
380 | 506 | ||
381 | unsigned int __kfifo_to_user_n(struct kfifo *fifo, | 507 | int __kfifo_from_user_r(struct __kfifo *fifo, const void __user *from, |
382 | void __user *to, unsigned int len, unsigned int reclen, | 508 | unsigned long len, unsigned int *copied, size_t recsize) |
383 | unsigned int recsize) | ||
384 | { | 509 | { |
385 | unsigned int ret, total; | 510 | unsigned long ret; |
386 | 511 | ||
387 | if (kfifo_len(fifo) < reclen + recsize) | 512 | len = __kfifo_max_r(len, recsize); |
388 | return len; | ||
389 | 513 | ||
390 | ret = __kfifo_to_user_data(fifo, to, reclen, recsize, &total); | 514 | if (len + recsize > kfifo_unused(fifo)) { |
515 | *copied = 0; | ||
516 | return 0; | ||
517 | } | ||
391 | 518 | ||
392 | if (likely(ret == 0)) | 519 | __kfifo_poke_n(fifo, len, recsize); |
393 | __kfifo_add_out(fifo, reclen + recsize); | ||
394 | 520 | ||
395 | return total; | 521 | ret = kfifo_copy_from_user(fifo, from, len, fifo->in + recsize, copied); |
522 | if (unlikely(ret)) { | ||
523 | *copied = 0; | ||
524 | return -EFAULT; | ||
525 | } | ||
526 | fifo->in += len + recsize; | ||
527 | return 0; | ||
396 | } | 528 | } |
397 | EXPORT_SYMBOL(__kfifo_to_user_n); | 529 | EXPORT_SYMBOL(__kfifo_from_user_r); |
398 | 530 | ||
399 | /** | 531 | int __kfifo_to_user_r(struct __kfifo *fifo, void __user *to, |
400 | * kfifo_to_user - gets data from the FIFO and write it to user space | 532 | unsigned long len, unsigned int *copied, size_t recsize) |
401 | * @fifo: the fifo to be used. | ||
402 | * @to: where the data must be copied. | ||
403 | * @len: the size of the destination buffer. | ||
404 | * @lenout: pointer to output variable with copied data | ||
405 | * | ||
406 | * This function copies at most @len bytes from the FIFO into the | ||
407 | * @to buffer and 0 or -EFAULT. | ||
408 | * | ||
409 | * Note that with only one concurrent reader and one concurrent | ||
410 | * writer, you don't need extra locking to use these functions. | ||
411 | */ | ||
412 | int kfifo_to_user(struct kfifo *fifo, | ||
413 | void __user *to, unsigned int len, unsigned *lenout) | ||
414 | { | 533 | { |
415 | int ret; | 534 | unsigned long ret; |
416 | len = min(kfifo_len(fifo), len); | 535 | unsigned int n; |
417 | ret = __kfifo_to_user_data(fifo, to, len, 0, lenout); | 536 | |
418 | __kfifo_add_out(fifo, *lenout); | 537 | if (fifo->in == fifo->out) { |
419 | return ret; | 538 | *copied = 0; |
539 | return 0; | ||
540 | } | ||
541 | |||
542 | n = __kfifo_peek_n(fifo, recsize); | ||
543 | if (len > n) | ||
544 | len = n; | ||
545 | |||
546 | ret = kfifo_copy_to_user(fifo, to, len, fifo->out + recsize, copied); | ||
547 | if (unlikely(ret)) { | ||
548 | *copied = 0; | ||
549 | return -EFAULT; | ||
550 | } | ||
551 | fifo->out += n + recsize; | ||
552 | return 0; | ||
420 | } | 553 | } |
421 | EXPORT_SYMBOL(kfifo_to_user); | 554 | EXPORT_SYMBOL(__kfifo_to_user_r); |
422 | 555 | ||
423 | unsigned int __kfifo_to_user_generic(struct kfifo *fifo, | 556 | unsigned int __kfifo_dma_in_prepare_r(struct __kfifo *fifo, |
424 | void __user *to, unsigned int len, unsigned int recsize, | 557 | struct scatterlist *sgl, int nents, unsigned int len, size_t recsize) |
425 | unsigned int *total) | ||
426 | { | 558 | { |
427 | return __kfifo_to_user_rec(fifo, to, len, recsize, total); | 559 | if (!nents) |
560 | BUG(); | ||
561 | |||
562 | len = __kfifo_max_r(len, recsize); | ||
563 | |||
564 | if (len + recsize > kfifo_unused(fifo)) | ||
565 | return 0; | ||
566 | |||
567 | return setup_sgl(fifo, sgl, nents, len, fifo->in + recsize); | ||
428 | } | 568 | } |
429 | EXPORT_SYMBOL(__kfifo_to_user_generic); | 569 | EXPORT_SYMBOL(__kfifo_dma_in_prepare_r); |
430 | 570 | ||
431 | unsigned int __kfifo_peek_generic(struct kfifo *fifo, unsigned int recsize) | 571 | void __kfifo_dma_in_finish_r(struct __kfifo *fifo, |
572 | unsigned int len, size_t recsize) | ||
432 | { | 573 | { |
433 | if (recsize == 0) | 574 | len = __kfifo_max_r(len, recsize); |
434 | return kfifo_avail(fifo); | 575 | __kfifo_poke_n(fifo, len, recsize); |
435 | 576 | fifo->in += len + recsize; | |
436 | return __kfifo_peek_n(fifo, recsize); | ||
437 | } | 577 | } |
438 | EXPORT_SYMBOL(__kfifo_peek_generic); | 578 | EXPORT_SYMBOL(__kfifo_dma_in_finish_r); |
439 | 579 | ||
440 | void __kfifo_skip_generic(struct kfifo *fifo, unsigned int recsize) | 580 | unsigned int __kfifo_dma_out_prepare_r(struct __kfifo *fifo, |
581 | struct scatterlist *sgl, int nents, unsigned int len, size_t recsize) | ||
441 | { | 582 | { |
442 | __kfifo_skip_rec(fifo, recsize); | 583 | if (!nents) |
584 | BUG(); | ||
585 | |||
586 | len = __kfifo_max_r(len, recsize); | ||
587 | |||
588 | if (len + recsize > fifo->in - fifo->out) | ||
589 | return 0; | ||
590 | |||
591 | return setup_sgl(fifo, sgl, nents, len, fifo->out + recsize); | ||
443 | } | 592 | } |
444 | EXPORT_SYMBOL(__kfifo_skip_generic); | 593 | EXPORT_SYMBOL(__kfifo_dma_out_prepare_r); |
594 | |||
595 | void __kfifo_dma_out_finish_r(struct __kfifo *fifo, size_t recsize) | ||
596 | { | ||
597 | unsigned int len; | ||
445 | 598 | ||
599 | len = __kfifo_peek_n(fifo, recsize); | ||
600 | fifo->out += len + recsize; | ||
601 | } | ||
602 | EXPORT_SYMBOL(__kfifo_dma_out_finish_r); | ||