LXR linux/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 <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