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 */
 20static inline unsigned int kfifo_unused(struct __kfifo *fifo)
 21{
 22	return (fifo->mask + 1) - (fifo->in - fifo->out);
 23}
 24
 25int __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}
 54EXPORT_SYMBOL(__kfifo_alloc);
 55
 56void __kfifo_free(struct __kfifo *fifo)
 57{
 58	kfree(fifo->data);
 59	fifo->in = 0;
 60	fifo->out = 0;
 61	fifo->esize = 0;
 62	fifo->data = NULL;
 63	fifo->mask = 0;
 64}
 65EXPORT_SYMBOL(__kfifo_free);
 66
 67int __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(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}
 88EXPORT_SYMBOL(__kfifo_init);
 89
 90static 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
114unsigned 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, buf, len, fifo->in);
124	fifo->in += len;
125	return len;
126}
127EXPORT_SYMBOL(__kfifo_in);
128
129static 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
153unsigned 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, buf, len, fifo->out);
163	return len;
164}
165EXPORT_SYMBOL(__kfifo_out_peek);
166
167unsigned 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}
178EXPORT_SYMBOL(__kfifo_out_linear);
179
180unsigned 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}
187EXPORT_SYMBOL(__kfifo_out);
188
189static 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(fifo->data + off, from, l);
207	if (unlikely(ret))
208		ret = DIV_ROUND_UP(ret + len - l, esize);
209	else {
210		ret = copy_from_user(fifo->data, from + l, 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
224int __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, 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}
248EXPORT_SYMBOL(__kfifo_from_user);
249
250static 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, fifo->data + off, l);
267	if (unlikely(ret))
268		ret = DIV_ROUND_UP(ret + len - l, esize);
269	else {
270		ret = copy_to_user(to + l, fifo->data, 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
284int __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, 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}
307EXPORT_SYMBOL(__kfifo_to_user);
308
309static 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(sgl, buf, 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
328static 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, off, nents, len_to_end, dma);
345	n += setup_sgl_buf(fifo, sgl + n, 0, nents - n, len - len_to_end, dma);
346
347	return n;
348}
349
350unsigned 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, fifo->in, dma);
361}
362EXPORT_SYMBOL(__kfifo_dma_in_prepare);
363
364unsigned 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, fifo->out, dma);
375}
376EXPORT_SYMBOL(__kfifo_dma_out_prepare);
377
378unsigned 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}
386EXPORT_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 */
394static 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 */
417static 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
428unsigned int __kfifo_len_r(struct __kfifo *fifo, size_t recsize)
429{
430	return __kfifo_peek_n(fifo, recsize);
431}
432EXPORT_SYMBOL(__kfifo_len_r);
433
434unsigned 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, len, recsize);
441
442	kfifo_copy_in(fifo, buf, len, fifo->in + recsize);
443	fifo->in += len + recsize;
444	return len;
445}
446EXPORT_SYMBOL(__kfifo_in_r);
447
448static 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, buf, len, fifo->out + recsize);
457	return len;
458}
459
460unsigned 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);
469}
470EXPORT_SYMBOL(__kfifo_out_peek_r);
471
472unsigned 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}
483EXPORT_SYMBOL(__kfifo_out_linear_r);
484
485unsigned 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);
494	fifo->out += n + recsize;
495	return len;
496}
497EXPORT_SYMBOL(__kfifo_out_r);
498
499void __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}
506EXPORT_SYMBOL(__kfifo_skip_r);
507
508int __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, len, recsize);
521
522	ret = kfifo_copy_from_user(fifo, from, len, fifo->in + recsize, copied);
523	if (unlikely(ret)) {
524		*copied = 0;
525		return -EFAULT;
526	}
527	fifo->in += len + recsize;
528	return 0;
529}
530EXPORT_SYMBOL(__kfifo_from_user_r);
531
532int __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, fifo->out + recsize, copied);
548	if (unlikely(ret)) {
549		*copied = 0;
550		return -EFAULT;
551	}
552	fifo->out += n + recsize;
553	return 0;
554}
555EXPORT_SYMBOL(__kfifo_to_user_r);
556
557unsigned 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, fifo->in + recsize, dma);
569}
570EXPORT_SYMBOL(__kfifo_dma_in_prepare_r);
571
572void __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, len, recsize);
577	fifo->in += len + recsize;
578}
579EXPORT_SYMBOL(__kfifo_dma_in_finish_r);
580
581unsigned 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, fifo->out + recsize, dma);
593}
594EXPORT_SYMBOL(__kfifo_dma_out_prepare_r);
595