linux/lib/kfifo.c
<<
>>
Prefs
   1/*
   2 * A generic kernel FIFO implementation
   3 *
   4 * Copyright (C) 2009/2010 Stefani Seibold <stefani@seibold.net>
   5 *
   6 * This program is free software; you can redistribute it and/or modify
   7 * it under the terms of the GNU General Public License as published by
   8 * the Free Software Foundation; either version 2 of the License, or
   9 * (at your option) any later version.
  10 *
  11 * This program is distributed in the hope that it will be useful,
  12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  14 * GNU General Public License for more details.
  15 *
  16 * You should have received a copy of the GNU General Public License
  17 * along with this program; if not, write to the Free Software
  18 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  19 *
  20 */
  21
  22#include <linux/kernel.h>
  23#include <linux/export.h>
  24#include <linux/slab.h>
  25#include <linux/err.h>
  26#include <linux/log2.h>
  27#include <linux/uaccess.h>
  28#include <linux/kfifo.h>
  29
  30/*
  31 * internal helper to calculate the unused elements in a fifo
  32 */
  33static inline unsigned int kfifo_unused(struct __kfifo *fifo)
  34{
  35        return (fifo->mask + 1) - (fifo->in - fifo->out);
  36}
  37
  38int __kfifo_alloc(struct __kfifo *fifo, unsigned int size,
  39                size_t esize, gfp_t gfp_mask)
  40{
  41        /*
  42         * round up to the next power of 2, since our 'let the indices
  43         * wrap' technique works only in this case.
  44         */
  45        size = roundup_pow_of_two(size);
  46
  47        fifo->in = 0;
  48        fifo->out = 0;
  49        fifo->esize = esize;
  50
  51        if (size < 2) {
  52                fifo->data = NULL;
  53                fifo->mask = 0;
  54                return -EINVAL;
  55        }
  56
  57        fifo->data = kmalloc_array(esize, size, gfp_mask);
  58
  59        if (!fifo->data) {
  60                fifo->mask = 0;
  61                return -ENOMEM;
  62        }
  63        fifo->mask = size - 1;
  64
  65        return 0;
  66}
  67EXPORT_SYMBOL(__kfifo_alloc);
  68
  69void __kfifo_free(struct __kfifo *fifo)
  70{
  71        kfree(fifo->data);
  72        fifo->in = 0;
  73        fifo->out = 0;
  74        fifo->esize = 0;
  75        fifo->data = NULL;
  76        fifo->mask = 0;
  77}
  78EXPORT_SYMBOL(__kfifo_free);
  79
  80int __kfifo_init(struct __kfifo *fifo, void *buffer,
  81                unsigned int size, size_t esize)
  82{
  83        size /= esize;
  84
  85        size = roundup_pow_of_two(size);
  86
  87        fifo->in = 0;
  88        fifo->out = 0;
  89        fifo->esize = esize;
  90        fifo->data = buffer;
  91
  92        if (size < 2) {
  93                fifo->mask = 0;
  94                return -EINVAL;
  95        }
  96        fifo->mask = size - 1;
  97
  98        return 0;
  99}
 100EXPORT_SYMBOL(__kfifo_init);
 101
 102static void kfifo_copy_in(struct __kfifo *fifo, const void *src,
 103                unsigned int len, unsigned int off)
 104{
 105        unsigned int size = fifo->mask + 1;
 106        unsigned int esize = fifo->esize;
 107        unsigned int l;
 108
 109        off &= fifo->mask;
 110        if (esize != 1) {
 111                off *= esize;
 112                size *= esize;
 113                len *= esize;
 114        }
 115        l = min(len, size - off);
 116
 117        memcpy(fifo->data + off, src, l);
 118        memcpy(fifo->data, src + l, len - l);
 119        /*
 120         * make sure that the data in the fifo is up to date before
 121         * incrementing the fifo->in index counter
 122         */
 123        smp_wmb();
 124}
 125
 126unsigned int __kfifo_in(struct __kfifo *fifo,
 127                const void *buf, unsigned int len)
 128{
 129        unsigned int l;
 130
 131        l = kfifo_unused(fifo);
 132        if (len > l)
 133                len = l;
 134
 135        kfifo_copy_in(fifo, buf, len, fifo->in);
 136        fifo->in += len;
 137        return len;
 138}
 139EXPORT_SYMBOL(__kfifo_in);
 140
 141static void kfifo_copy_out(struct __kfifo *fifo, void *dst,
 142                unsigned int len, unsigned int off)
 143{
 144        unsigned int size = fifo->mask + 1;
 145        unsigned int esize = fifo->esize;
 146        unsigned int l;
 147
 148        off &= fifo->mask;
 149        if (esize != 1) {
 150                off *= esize;
 151                size *= esize;
 152                len *= esize;
 153        }
 154        l = min(len, size - off);
 155
 156        memcpy(dst, fifo->data + off, l);
 157        memcpy(dst + l, fifo->data, len - l);
 158        /*
 159         * make sure that the data is copied before
 160         * incrementing the fifo->out index counter
 161         */
 162        smp_wmb();
 163}
 164
 165unsigned int __kfifo_out_peek(struct __kfifo *fifo,
 166                void *buf, unsigned int len)
 167{
 168        unsigned int l;
 169
 170        l = fifo->in - fifo->out;
 171        if (len > l)
 172                len = l;
 173
 174        kfifo_copy_out(fifo, buf, len, fifo->out);
 175        return len;
 176}
 177EXPORT_SYMBOL(__kfifo_out_peek);
 178
 179unsigned int __kfifo_out(struct __kfifo *fifo,
 180                void *buf, unsigned int len)
 181{
 182        len = __kfifo_out_peek(fifo, buf, len);
 183        fifo->out += len;
 184        return len;
 185}
 186EXPORT_SYMBOL(__kfifo_out);
 187
 188static unsigned long kfifo_copy_from_user(struct __kfifo *fifo,
 189        const void __user *from, unsigned int len, unsigned int off,
 190        unsigned int *copied)
 191{
 192        unsigned int size = fifo->mask + 1;
 193        unsigned int esize = fifo->esize;
 194        unsigned int l;
 195        unsigned long ret;
 196
 197        off &= fifo->mask;
 198        if (esize != 1) {
 199                off *= esize;
 200                size *= esize;
 201                len *= esize;
 202        }
 203        l = min(len, size - off);
 204
 205        ret = copy_from_user(fifo->data + off, from, l);
 206        if (unlikely(ret))
 207                ret = DIV_ROUND_UP(ret + len - l, esize);
 208        else {
 209                ret = copy_from_user(fifo->data, from + l, len - l);
 210                if (unlikely(ret))
 211                        ret = DIV_ROUND_UP(ret, esize);
 212        }
 213        /*
 214         * make sure that the data in the fifo is up to date before
 215         * incrementing the fifo->in index counter
 216         */
 217        smp_wmb();
 218        *copied = len - ret * esize;
 219        /* return the number of elements which are not copied */
 220        return ret;
 221}
 222
 223int __kfifo_from_user(struct __kfifo *fifo, const void __user *from,
 224                unsigned long len, unsigned int *copied)
 225{
 226        unsigned int l;
 227        unsigned long ret;
 228        unsigned int esize = fifo->esize;
 229        int err;
 230
 231        if (esize != 1)
 232                len /= esize;
 233
 234        l = kfifo_unused(fifo);
 235        if (len > l)
 236                len = l;
 237
 238        ret = kfifo_copy_from_user(fifo, from, len, fifo->in, copied);
 239        if (unlikely(ret)) {
 240                len -= ret;
 241                err = -EFAULT;
 242        } else
 243                err = 0;
 244        fifo->in += len;
 245        return err;
 246}
 247EXPORT_SYMBOL(__kfifo_from_user);
 248
 249static unsigned long kfifo_copy_to_user(struct __kfifo *fifo, void __user *to,
 250                unsigned int len, unsigned int off, unsigned int *copied)
 251{
 252        unsigned int l;
 253        unsigned long ret;
 254        unsigned int size = fifo->mask + 1;
 255        unsigned int esize = fifo->esize;
 256
 257        off &= fifo->mask;
 258        if (esize != 1) {
 259                off *= esize;
 260                size *= esize;
 261                len *= esize;
 262        }
 263        l = min(len, size - off);
 264
 265        ret = copy_to_user(to, fifo->data + off, l);
 266        if (unlikely(ret))
 267                ret = DIV_ROUND_UP(ret + len - l, esize);
 268        else {
 269                ret = copy_to_user(to + l, fifo->data, len - l);
 270                if (unlikely(ret))
 271                        ret = DIV_ROUND_UP(ret, esize);
 272        }
 273        /*
 274         * make sure that the data is copied before
 275         * incrementing the fifo->out index counter
 276         */
 277        smp_wmb();
 278        *copied = len - ret * esize;
 279        /* return the number of elements which are not copied */
 280        return ret;
 281}
 282
 283int __kfifo_to_user(struct __kfifo *fifo, void __user *to,
 284                unsigned long len, unsigned int *copied)
 285{
 286        unsigned int l;
 287        unsigned long ret;
 288        unsigned int esize = fifo->esize;
 289        int err;
 290
 291        if (esize != 1)
 292                len /= esize;
 293
 294        l = fifo->in - fifo->out;
 295        if (len > l)
 296                len = l;
 297        ret = kfifo_copy_to_user(fifo, to, len, fifo->out, copied);
 298        if (unlikely(ret)) {
 299                len -= ret;
 300                err = -EFAULT;
 301        } else
 302                err = 0;
 303        fifo->out += len;
 304        return err;
 305}
 306EXPORT_SYMBOL(__kfifo_to_user);
 307
 308static int setup_sgl_buf(struct scatterlist *sgl, void *buf,
 309                int nents, unsigned int len)
 310{
 311        int n;
 312        unsigned int l;
 313        unsigned int off;
 314        struct page *page;
 315
 316        if (!nents)
 317                return 0;
 318
 319        if (!len)
 320                return 0;
 321
 322        n = 0;
 323        page = virt_to_page(buf);
 324        off = offset_in_page(buf);
 325        l = 0;
 326
 327        while (len >= l + PAGE_SIZE - off) {
 328                struct page *npage;
 329
 330                l += PAGE_SIZE;
 331                buf += PAGE_SIZE;
 332                npage = virt_to_page(buf);
 333                if (page_to_phys(page) != page_to_phys(npage) - l) {
 334                        sg_set_page(sgl, page, l - off, off);
 335                        sgl = sg_next(sgl);
 336                        if (++n == nents || sgl == NULL)
 337                                return n;
 338                        page = npage;
 339                        len -= l - off;
 340                        l = off = 0;
 341                }
 342        }
 343        sg_set_page(sgl, page, len, off);
 344        return n + 1;
 345}
 346
 347static unsigned int setup_sgl(struct __kfifo *fifo, struct scatterlist *sgl,
 348                int nents, unsigned int len, unsigned int off)
 349{
 350        unsigned int size = fifo->mask + 1;
 351        unsigned int esize = fifo->esize;
 352        unsigned int l;
 353        unsigned int n;
 354
 355        off &= fifo->mask;
 356        if (esize != 1) {
 357                off *= esize;
 358                size *= esize;
 359                len *= esize;
 360        }
 361        l = min(len, size - off);
 362
 363        n = setup_sgl_buf(sgl, fifo->data + off, nents, l);
 364        n += setup_sgl_buf(sgl + n, fifo->data, nents - n, len - l);
 365
 366        return n;
 367}
 368
 369unsigned int __kfifo_dma_in_prepare(struct __kfifo *fifo,
 370                struct scatterlist *sgl, int nents, unsigned int len)
 371{
 372        unsigned int l;
 373
 374        l = kfifo_unused(fifo);
 375        if (len > l)
 376                len = l;
 377
 378        return setup_sgl(fifo, sgl, nents, len, fifo->in);
 379}
 380EXPORT_SYMBOL(__kfifo_dma_in_prepare);
 381
 382unsigned int __kfifo_dma_out_prepare(struct __kfifo *fifo,
 383                struct scatterlist *sgl, int nents, unsigned int len)
 384{
 385        unsigned int l;
 386
 387        l = fifo->in - fifo->out;
 388        if (len > l)
 389                len = l;
 390
 391        return setup_sgl(fifo, sgl, nents, len, fifo->out);
 392}
 393EXPORT_SYMBOL(__kfifo_dma_out_prepare);
 394
 395unsigned int __kfifo_max_r(unsigned int len, size_t recsize)
 396{
 397        unsigned int max = (1 << (recsize << 3)) - 1;
 398
 399        if (len > max)
 400                return max;
 401        return len;
 402}
 403EXPORT_SYMBOL(__kfifo_max_r);
 404
 405#define __KFIFO_PEEK(data, out, mask) \
 406        ((data)[(out) & (mask)])
 407/*
 408 * __kfifo_peek_n internal helper function for determinate the length of
 409 * the next record in the fifo
 410 */
 411static unsigned int __kfifo_peek_n(struct __kfifo *fifo, size_t recsize)
 412{
 413        unsigned int l;
 414        unsigned int mask = fifo->mask;
 415        unsigned char *data = fifo->data;
 416
 417        l = __KFIFO_PEEK(data, fifo->out, mask);
 418
 419        if (--recsize)
 420                l |= __KFIFO_PEEK(data, fifo->out + 1, mask) << 8;
 421
 422        return l;
 423}
 424
 425#define __KFIFO_POKE(data, in, mask, val) \
 426        ( \
 427        (data)[(in) & (mask)] = (unsigned char)(val) \
 428        )
 429
 430/*
 431 * __kfifo_poke_n internal helper function for storeing the length of
 432 * the record into the fifo
 433 */
 434static void __kfifo_poke_n(struct __kfifo *fifo, unsigned int n, size_t recsize)
 435{
 436        unsigned int mask = fifo->mask;
 437        unsigned char *data = fifo->data;
 438
 439        __KFIFO_POKE(data, fifo->in, mask, n);
 440
 441        if (recsize > 1)
 442                __KFIFO_POKE(data, fifo->in + 1, mask, n >> 8);
 443}
 444
 445unsigned int __kfifo_len_r(struct __kfifo *fifo, size_t recsize)
 446{
 447        return __kfifo_peek_n(fifo, recsize);
 448}
 449EXPORT_SYMBOL(__kfifo_len_r);
 450
 451unsigned int __kfifo_in_r(struct __kfifo *fifo, const void *buf,
 452                unsigned int len, size_t recsize)
 453{
 454        if (len + recsize > kfifo_unused(fifo))
 455                return 0;
 456
 457        __kfifo_poke_n(fifo, len, recsize);
 458
 459        kfifo_copy_in(fifo, buf, len, fifo->in + recsize);
 460        fifo->in += len + recsize;
 461        return len;
 462}
 463EXPORT_SYMBOL(__kfifo_in_r);
 464
 465static unsigned int kfifo_out_copy_r(struct __kfifo *fifo,
 466        void *buf, unsigned int len, size_t recsize, unsigned int *n)
 467{
 468        *n = __kfifo_peek_n(fifo, recsize);
 469
 470        if (len > *n)
 471                len = *n;
 472
 473        kfifo_copy_out(fifo, buf, len, fifo->out + recsize);
 474        return len;
 475}
 476
 477unsigned int __kfifo_out_peek_r(struct __kfifo *fifo, void *buf,
 478                unsigned int len, size_t recsize)
 479{
 480        unsigned int n;
 481
 482        if (fifo->in == fifo->out)
 483                return 0;
 484
 485        return kfifo_out_copy_r(fifo, buf, len, recsize, &n);
 486}
 487EXPORT_SYMBOL(__kfifo_out_peek_r);
 488
 489unsigned int __kfifo_out_r(struct __kfifo *fifo, void *buf,
 490                unsigned int len, size_t recsize)
 491{
 492        unsigned int n;
 493
 494        if (fifo->in == fifo->out)
 495                return 0;
 496
 497        len = kfifo_out_copy_r(fifo, buf, len, recsize, &n);
 498        fifo->out += n + recsize;
 499        return len;
 500}
 501EXPORT_SYMBOL(__kfifo_out_r);
 502
 503void __kfifo_skip_r(struct __kfifo *fifo, size_t recsize)
 504{
 505        unsigned int n;
 506
 507        n = __kfifo_peek_n(fifo, recsize);
 508        fifo->out += n + recsize;
 509}
 510EXPORT_SYMBOL(__kfifo_skip_r);
 511
 512int __kfifo_from_user_r(struct __kfifo *fifo, const void __user *from,
 513        unsigned long len, unsigned int *copied, size_t recsize)
 514{
 515        unsigned long ret;
 516
 517        len = __kfifo_max_r(len, recsize);
 518
 519        if (len + recsize > kfifo_unused(fifo)) {
 520                *copied = 0;
 521                return 0;
 522        }
 523
 524        __kfifo_poke_n(fifo, len, recsize);
 525
 526        ret = kfifo_copy_from_user(fifo, from, len, fifo->in + recsize, copied);
 527        if (unlikely(ret)) {
 528                *copied = 0;
 529                return -EFAULT;
 530        }
 531        fifo->in += len + recsize;
 532        return 0;
 533}
 534EXPORT_SYMBOL(__kfifo_from_user_r);
 535
 536int __kfifo_to_user_r(struct __kfifo *fifo, void __user *to,
 537        unsigned long len, unsigned int *copied, size_t recsize)
 538{
 539        unsigned long ret;
 540        unsigned int n;
 541
 542        if (fifo->in == fifo->out) {
 543                *copied = 0;
 544                return 0;
 545        }
 546
 547        n = __kfifo_peek_n(fifo, recsize);
 548        if (len > n)
 549                len = n;
 550
 551        ret = kfifo_copy_to_user(fifo, to, len, fifo->out + recsize, copied);
 552        if (unlikely(ret)) {
 553                *copied = 0;
 554                return -EFAULT;
 555        }
 556        fifo->out += n + recsize;
 557        return 0;
 558}
 559EXPORT_SYMBOL(__kfifo_to_user_r);
 560
 561unsigned int __kfifo_dma_in_prepare_r(struct __kfifo *fifo,
 562        struct scatterlist *sgl, int nents, unsigned int len, size_t recsize)
 563{
 564        BUG_ON(!nents);
 565
 566        len = __kfifo_max_r(len, recsize);
 567
 568        if (len + recsize > kfifo_unused(fifo))
 569                return 0;
 570
 571        return setup_sgl(fifo, sgl, nents, len, fifo->in + recsize);
 572}
 573EXPORT_SYMBOL(__kfifo_dma_in_prepare_r);
 574
 575void __kfifo_dma_in_finish_r(struct __kfifo *fifo,
 576        unsigned int len, size_t recsize)
 577{
 578        len = __kfifo_max_r(len, recsize);
 579        __kfifo_poke_n(fifo, len, recsize);
 580        fifo->in += len + recsize;
 581}
 582EXPORT_SYMBOL(__kfifo_dma_in_finish_r);
 583
 584unsigned int __kfifo_dma_out_prepare_r(struct __kfifo *fifo,
 585        struct scatterlist *sgl, int nents, unsigned int len, size_t recsize)
 586{
 587        BUG_ON(!nents);
 588
 589        len = __kfifo_max_r(len, recsize);
 590
 591        if (len + recsize > fifo->in - fifo->out)
 592                return 0;
 593
 594        return setup_sgl(fifo, sgl, nents, len, fifo->out + recsize);
 595}
 596EXPORT_SYMBOL(__kfifo_dma_out_prepare_r);
 597
 598void __kfifo_dma_out_finish_r(struct __kfifo *fifo, size_t recsize)
 599{
 600        unsigned int len;
 601
 602        len = __kfifo_peek_n(fifo, recsize);
 603        fifo->out += len + recsize;
 604}
 605EXPORT_SYMBOL(__kfifo_dma_out_finish_r);
 606