Linux Kernel  3.7.1
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
kfifo.c
Go to the documentation of this file.
1 /*
2  * A generic kernel FIFO implementation
3  *
4  * Copyright (C) 2009/2010 Stefani Seibold <[email protected]>
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
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 }
577 
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 }
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 }
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 }