kernel/kfifo.c

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