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