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