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 */
19static inline unsigned int kfifo_unused(struct __kfifo *fifo)
20{
21	return (fifo->mask + 1) - (fifo->in - fifo->out);
22}
23
24int __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(esize, size, 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}
53EXPORT_SYMBOL(__kfifo_alloc);
54
55void __kfifo_free(struct __kfifo *fifo)
56{
57	kfree(fifo->data);
58	fifo->in = 0;
59	fifo->out = 0;
60	fifo->esize = 0;
61	fifo->data = NULL;
62	fifo->mask = 0;
63}
64EXPORT_SYMBOL(__kfifo_free);
65
66int __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(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}
87EXPORT_SYMBOL(__kfifo_init);
88
89static 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
113unsigned 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, buf, len, fifo->in);
123	fifo->in += len;
124	return len;
125}
126EXPORT_SYMBOL(__kfifo_in);
127
128static 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
152unsigned 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, buf, len, fifo->out);
162	return len;
163}
164EXPORT_SYMBOL(__kfifo_out_peek);
165
166unsigned 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}
173EXPORT_SYMBOL(__kfifo_out);
174
175static 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(fifo->data + off, from, l);
193	if (unlikely(ret))
194		ret = DIV_ROUND_UP(ret + len - l, esize);
195	else {
196		ret = copy_from_user(fifo->data, from + l, 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
210int __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, 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}
234EXPORT_SYMBOL(__kfifo_from_user);
235
236static 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, fifo->data + off, l);
253	if (unlikely(ret))
254		ret = DIV_ROUND_UP(ret + len - l, esize);
255	else {
256		ret = copy_to_user(to + l, fifo->data, 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
270int __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, 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}
293EXPORT_SYMBOL(__kfifo_to_user);
294
295static 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(sgl, page, l - off, 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(sgl, page, len, off);
331	return n + 1;
332}
333
334static 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, fifo->data + off, nents, l);
351	n += setup_sgl_buf(sgl + n, fifo->data, nents - n, len - l);
352
353	return n;
354}
355
356unsigned 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, fifo->in);
366}
367EXPORT_SYMBOL(__kfifo_dma_in_prepare);
368
369unsigned 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, fifo->out);
379}
380EXPORT_SYMBOL(__kfifo_dma_out_prepare);
381
382unsigned 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}
390EXPORT_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 */
398static 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 */
421static 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
432unsigned int __kfifo_len_r(struct __kfifo *fifo, size_t recsize)
433{
434	return __kfifo_peek_n(fifo, recsize);
435}
436EXPORT_SYMBOL(__kfifo_len_r);
437
438unsigned 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, len, recsize);
445
446	kfifo_copy_in(fifo, buf, len, fifo->in + recsize);
447	fifo->in += len + recsize;
448	return len;
449}
450EXPORT_SYMBOL(__kfifo_in_r);
451
452static 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, buf, len, fifo->out + recsize);
461	return len;
462}
463
464unsigned 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);
473}
474EXPORT_SYMBOL(__kfifo_out_peek_r);
475
476unsigned 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);
485	fifo->out += n + recsize;
486	return len;
487}
488EXPORT_SYMBOL(__kfifo_out_r);
489
490void __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}
497EXPORT_SYMBOL(__kfifo_skip_r);
498
499int __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, len, recsize);
512
513	ret = kfifo_copy_from_user(fifo, from, len, fifo->in + recsize, copied);
514	if (unlikely(ret)) {
515		*copied = 0;
516		return -EFAULT;
517	}
518	fifo->in += len + recsize;
519	return 0;
520}
521EXPORT_SYMBOL(__kfifo_from_user_r);
522
523int __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, fifo->out + recsize, copied);
539	if (unlikely(ret)) {
540		*copied = 0;
541		return -EFAULT;
542	}
543	fifo->out += n + recsize;
544	return 0;
545}
546EXPORT_SYMBOL(__kfifo_to_user_r);
547
548unsigned 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, fifo->in + recsize);
559}
560EXPORT_SYMBOL(__kfifo_dma_in_prepare_r);
561
562void __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, len, recsize);
567	fifo->in += len + recsize;
568}
569EXPORT_SYMBOL(__kfifo_dma_in_finish_r);
570
571unsigned 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, fifo->out + recsize);
582}
583EXPORT_SYMBOL(__kfifo_dma_out_prepare_r);
584
585void __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}
592EXPORT_SYMBOL(__kfifo_dma_out_finish_r);
593