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