lib/kfifo.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * A generic kernel FIFO implementation
   4  *
   5  * Copyright (C) 2009/2010 Stefani Seibold <[email protected]>
   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(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(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, buf, len, 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, buf, len, 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(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
 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, 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, 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
 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, 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(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
 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, 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
 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, 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, 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, len, recsize);
 441
 442         kfifo_copy_in(fifo, buf, len, 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, buf, len, 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);
 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);
 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, 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 }
 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, 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, 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, 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, fifo->out + recsize, dma);
 593 }
 594 EXPORT_SYMBOL(__kfifo_dma_out_prepare_r);
 595