Root/
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 | 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 | EXPORT_SYMBOL(__kfifo_max_r); |
406 | |
407 | #define __KFIFO_PEEK(data, out, mask) \ |
408 | ((data)[(out) & (mask)]) |
409 | /* |
410 | * __kfifo_peek_n internal helper function for determinate the length of |
411 | * the next record in the fifo |
412 | */ |
413 | static unsigned int __kfifo_peek_n(struct __kfifo *fifo, size_t recsize) |
414 | { |
415 | unsigned int l; |
416 | unsigned int mask = fifo->mask; |
417 | unsigned char *data = fifo->data; |
418 | |
419 | l = __KFIFO_PEEK(data, fifo->out, mask); |
420 | |
421 | if (--recsize) |
422 | l |= __KFIFO_PEEK(data, fifo->out + 1, mask) << 8; |
423 | |
424 | return l; |
425 | } |
426 | |
427 | #define __KFIFO_POKE(data, in, mask, val) \ |
428 | ( \ |
429 | (data)[(in) & (mask)] = (unsigned char)(val) \ |
430 | ) |
431 | |
432 | /* |
433 | * __kfifo_poke_n internal helper function for storeing the length of |
434 | * the record into the fifo |
435 | */ |
436 | static void __kfifo_poke_n(struct __kfifo *fifo, unsigned int n, size_t recsize) |
437 | { |
438 | unsigned int mask = fifo->mask; |
439 | unsigned char *data = fifo->data; |
440 | |
441 | __KFIFO_POKE(data, fifo->in, mask, n); |
442 | |
443 | if (recsize > 1) |
444 | __KFIFO_POKE(data, fifo->in + 1, mask, n >> 8); |
445 | } |
446 | |
447 | unsigned int __kfifo_len_r(struct __kfifo *fifo, size_t recsize) |
448 | { |
449 | return __kfifo_peek_n(fifo, recsize); |
450 | } |
451 | EXPORT_SYMBOL(__kfifo_len_r); |
452 | |
453 | unsigned int __kfifo_in_r(struct __kfifo *fifo, const void *buf, |
454 | unsigned int len, size_t recsize) |
455 | { |
456 | if (len + recsize > kfifo_unused(fifo)) |
457 | return 0; |
458 | |
459 | __kfifo_poke_n(fifo, len, recsize); |
460 | |
461 | kfifo_copy_in(fifo, buf, len, fifo->in + recsize); |
462 | fifo->in += len + recsize; |
463 | return len; |
464 | } |
465 | EXPORT_SYMBOL(__kfifo_in_r); |
466 | |
467 | static unsigned int kfifo_out_copy_r(struct __kfifo *fifo, |
468 | void *buf, unsigned int len, size_t recsize, unsigned int *n) |
469 | { |
470 | *n = __kfifo_peek_n(fifo, recsize); |
471 | |
472 | if (len > *n) |
473 | len = *n; |
474 | |
475 | kfifo_copy_out(fifo, buf, len, fifo->out + recsize); |
476 | return len; |
477 | } |
478 | |
479 | unsigned int __kfifo_out_peek_r(struct __kfifo *fifo, void *buf, |
480 | unsigned int len, size_t recsize) |
481 | { |
482 | unsigned int n; |
483 | |
484 | if (fifo->in == fifo->out) |
485 | return 0; |
486 | |
487 | return kfifo_out_copy_r(fifo, buf, len, recsize, &n); |
488 | } |
489 | EXPORT_SYMBOL(__kfifo_out_peek_r); |
490 | |
491 | unsigned int __kfifo_out_r(struct __kfifo *fifo, void *buf, |
492 | unsigned int len, size_t recsize) |
493 | { |
494 | unsigned int n; |
495 | |
496 | if (fifo->in == fifo->out) |
497 | return 0; |
498 | |
499 | len = kfifo_out_copy_r(fifo, buf, len, recsize, &n); |
500 | fifo->out += n + recsize; |
501 | return len; |
502 | } |
503 | EXPORT_SYMBOL(__kfifo_out_r); |
504 | |
505 | void __kfifo_skip_r(struct __kfifo *fifo, size_t recsize) |
506 | { |
507 | unsigned int n; |
508 | |
509 | n = __kfifo_peek_n(fifo, recsize); |
510 | fifo->out += n + recsize; |
511 | } |
512 | EXPORT_SYMBOL(__kfifo_skip_r); |
513 | |
514 | int __kfifo_from_user_r(struct __kfifo *fifo, const void __user *from, |
515 | unsigned long len, unsigned int *copied, size_t recsize) |
516 | { |
517 | unsigned long ret; |
518 | |
519 | len = __kfifo_max_r(len, recsize); |
520 | |
521 | if (len + recsize > kfifo_unused(fifo)) { |
522 | *copied = 0; |
523 | return 0; |
524 | } |
525 | |
526 | __kfifo_poke_n(fifo, len, recsize); |
527 | |
528 | ret = kfifo_copy_from_user(fifo, from, len, fifo->in + recsize, copied); |
529 | if (unlikely(ret)) { |
530 | *copied = 0; |
531 | return -EFAULT; |
532 | } |
533 | fifo->in += len + recsize; |
534 | return 0; |
535 | } |
536 | EXPORT_SYMBOL(__kfifo_from_user_r); |
537 | |
538 | int __kfifo_to_user_r(struct __kfifo *fifo, void __user *to, |
539 | unsigned long len, unsigned int *copied, size_t recsize) |
540 | { |
541 | unsigned long ret; |
542 | unsigned int n; |
543 | |
544 | if (fifo->in == fifo->out) { |
545 | *copied = 0; |
546 | return 0; |
547 | } |
548 | |
549 | n = __kfifo_peek_n(fifo, recsize); |
550 | if (len > n) |
551 | len = n; |
552 | |
553 | ret = kfifo_copy_to_user(fifo, to, len, fifo->out + recsize, copied); |
554 | if (unlikely(ret)) { |
555 | *copied = 0; |
556 | return -EFAULT; |
557 | } |
558 | fifo->out += n + recsize; |
559 | return 0; |
560 | } |
561 | EXPORT_SYMBOL(__kfifo_to_user_r); |
562 | |
563 | unsigned int __kfifo_dma_in_prepare_r(struct __kfifo *fifo, |
564 | struct scatterlist *sgl, int nents, unsigned int len, size_t recsize) |
565 | { |
566 | if (!nents) |
567 | BUG(); |
568 | |
569 | len = __kfifo_max_r(len, recsize); |
570 | |
571 | if (len + recsize > kfifo_unused(fifo)) |
572 | return 0; |
573 | |
574 | return setup_sgl(fifo, sgl, nents, len, fifo->in + recsize); |
575 | } |
576 | EXPORT_SYMBOL(__kfifo_dma_in_prepare_r); |
577 | |
578 | void __kfifo_dma_in_finish_r(struct __kfifo *fifo, |
579 | unsigned int len, size_t recsize) |
580 | { |
581 | len = __kfifo_max_r(len, recsize); |
582 | __kfifo_poke_n(fifo, len, recsize); |
583 | fifo->in += len + recsize; |
584 | } |
585 | EXPORT_SYMBOL(__kfifo_dma_in_finish_r); |
586 | |
587 | unsigned int __kfifo_dma_out_prepare_r(struct __kfifo *fifo, |
588 | struct scatterlist *sgl, int nents, unsigned int len, size_t recsize) |
589 | { |
590 | if (!nents) |
591 | BUG(); |
592 | |
593 | len = __kfifo_max_r(len, recsize); |
594 | |
595 | if (len + recsize > fifo->in - fifo->out) |
596 | return 0; |
597 | |
598 | return setup_sgl(fifo, sgl, nents, len, fifo->out + recsize); |
599 | } |
600 | EXPORT_SYMBOL(__kfifo_dma_out_prepare_r); |
601 | |
602 | void __kfifo_dma_out_finish_r(struct __kfifo *fifo, size_t recsize) |
603 | { |
604 | unsigned int len; |
605 | |
606 | len = __kfifo_peek_n(fifo, recsize); |
607 | fifo->out += len + recsize; |
608 | } |
609 | EXPORT_SYMBOL(__kfifo_dma_out_finish_r); |
610 |
Branches:
ben-wpan
ben-wpan-stefan
javiroman/ks7010
jz-2.6.34
jz-2.6.34-rc5
jz-2.6.34-rc6
jz-2.6.34-rc7
jz-2.6.35
jz-2.6.36
jz-2.6.37
jz-2.6.38
jz-2.6.39
jz-3.0
jz-3.1
jz-3.11
jz-3.12
jz-3.13
jz-3.15
jz-3.16
jz-3.18-dt
jz-3.2
jz-3.3
jz-3.4
jz-3.5
jz-3.6
jz-3.6-rc2-pwm
jz-3.9
jz-3.9-clk
jz-3.9-rc8
jz47xx
jz47xx-2.6.38
master
Tags:
od-2011-09-04
od-2011-09-18
v2.6.34-rc5
v2.6.34-rc6
v2.6.34-rc7
v3.9