596 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * A generic kernel FIFO implementation4 *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 fifo19 */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 indices30 * 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 before109 * incrementing the fifo->in index counter110 */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 before148 * incrementing the fifo->out index counter149 */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 before216 * incrementing the fifo->in index counter217 */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 } else244 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 before276 * incrementing the fifo->out index counter277 */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 } else303 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 of392 * the next record in the fifo393 */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 of415 * the record into the fifo416 */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 596