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         if (n)
 369                 sg_mark_end(sgl + n - 1);
 370         return n;
 371 }
 372
 373 unsigned int __kfifo_dma_in_prepare(struct __kfifo *fifo,
 374                 struct scatterlist *sgl, int nents, unsigned int len)
 375 {
 376         unsigned int l;
 377
 378         l = kfifo_unused(fifo);
 379         if (len > l)
 380                 len = l;
 381
 382         return setup_sgl(fifo, sgl, nents, len, fifo->in);
 383 }
 384 EXPORT_SYMBOL(__kfifo_dma_in_prepare);
 385
 386 unsigned int __kfifo_dma_out_prepare(struct __kfifo *fifo,
 387                 struct scatterlist *sgl, int nents, unsigned int len)
 388 {
 389         unsigned int l;
 390
 391         l = fifo->in - fifo->out;
 392         if (len > l)
 393                 len = l;
 394
 395         return setup_sgl(fifo, sgl, nents, len, fifo->out);
 396 }
 397 EXPORT_SYMBOL(__kfifo_dma_out_prepare);
 398
 399 unsigned int __kfifo_max_r(unsigned int len, size_t recsize)
 400 {
 401         unsigned int max = (1 << (recsize << 3)) - 1;
 402
 403         if (len > max)
 404                 return max;
 405         return len;
 406 }
 407
 408 #define __KFIFO_PEEK(data, out, mask) \
 409         ((data)[(out) & (mask)])
 410 /*
 411  * __kfifo_peek_n internal helper function for determinate the length of
 412  * the next record in the fifo
 413  */
 414 static unsigned int __kfifo_peek_n(struct __kfifo *fifo, size_t recsize)
 415 {
 416         unsigned int l;
 417         unsigned int mask = fifo->mask;
 418         unsigned char *data = fifo->data;
 419
 420         l = __KFIFO_PEEK(data, fifo->out, mask);
 421
 422         if (--recsize)
 423                 l |= __KFIFO_PEEK(data, fifo->out + 1, mask) << 8;
 424
 425         return l;
 426 }
 427
 428 #define __KFIFO_POKE(data, in, mask, val) \
 429         ( \
 430         (data)[(in) & (mask)] = (unsigned char)(val) \
 431         )
 432
 433 /*
 434  * __kfifo_poke_n internal helper function for storeing the length of
 435  * the record into the fifo
 436  */
 437 static void __kfifo_poke_n(struct __kfifo *fifo, unsigned int n, size_t recsize)
 438 {
 439         unsigned int mask = fifo->mask;
 440         unsigned char *data = fifo->data;
 441
 442         __KFIFO_POKE(data, fifo->in, mask, n);
 443
 444         if (recsize > 1)
 445                 __KFIFO_POKE(data, fifo->in + 1, mask, n >> 8);
 446 }
 447
 448 unsigned int __kfifo_len_r(struct __kfifo *fifo, size_t recsize)
 449 {
 450         return __kfifo_peek_n(fifo, recsize);
 451 }
 452 EXPORT_SYMBOL(__kfifo_len_r);
 453
 454 unsigned int __kfifo_in_r(struct __kfifo *fifo, const void *buf,
 455                 unsigned int len, size_t recsize)
 456 {
 457         if (len + recsize > kfifo_unused(fifo))
 458                 return 0;
 459
 460         __kfifo_poke_n(fifo, len, recsize);
 461
 462         kfifo_copy_in(fifo, buf, len, fifo->in + recsize);
 463         fifo->in += len + recsize;
 464         return len;
 465 }
 466 EXPORT_SYMBOL(__kfifo_in_r);
 467
 468 static unsigned int kfifo_out_copy_r(struct __kfifo *fifo,
 469         void *buf, unsigned int len, size_t recsize, unsigned int *n)
 470 {
 471         *n = __kfifo_peek_n(fifo, recsize);
 472
 473         if (len > *n)
 474                 len = *n;
 475
 476         kfifo_copy_out(fifo, buf, len, fifo->out + recsize);
 477         return len;
 478 }
 479
 480 unsigned int __kfifo_out_peek_r(struct __kfifo *fifo, void *buf,
 481                 unsigned int len, size_t recsize)
 482 {
 483         unsigned int n;
 484
 485         if (fifo->in == fifo->out)
 486                 return 0;
 487
 488         return kfifo_out_copy_r(fifo, buf, len, recsize, &n);
 489 }
 490 EXPORT_SYMBOL(__kfifo_out_peek_r);
 491
 492 unsigned int __kfifo_out_r(struct __kfifo *fifo, void *buf,
 493                 unsigned int len, size_t recsize)
 494 {
 495         unsigned int n;
 496
 497         if (fifo->in == fifo->out)
 498                 return 0;
 499
 500         len = kfifo_out_copy_r(fifo, buf, len, recsize, &n);
 501         fifo->out += n + recsize;
 502         return len;
 503 }
 504 EXPORT_SYMBOL(__kfifo_out_r);
 505
 506 void __kfifo_skip_r(struct __kfifo *fifo, size_t recsize)
 507 {
 508         unsigned int n;
 509
 510         n = __kfifo_peek_n(fifo, recsize);
 511         fifo->out += n + recsize;
 512 }
 513 EXPORT_SYMBOL(__kfifo_skip_r);
 514
 515 int __kfifo_from_user_r(struct __kfifo *fifo, const void __user *from,
 516         unsigned long len, unsigned int *copied, size_t recsize)
 517 {
 518         unsigned long ret;
 519
 520         len = __kfifo_max_r(len, recsize);
 521
 522         if (len + recsize > kfifo_unused(fifo)) {
 523                 *copied = 0;
 524                 return 0;
 525         }
 526
 527         __kfifo_poke_n(fifo, len, recsize);
 528
 529         ret = kfifo_copy_from_user(fifo, from, len, fifo->in + recsize, copied);
 530         if (unlikely(ret)) {
 531                 *copied = 0;
 532                 return -EFAULT;
 533         }
 534         fifo->in += len + recsize;
 535         return 0;
 536 }
 537 EXPORT_SYMBOL(__kfifo_from_user_r);
 538
 539 int __kfifo_to_user_r(struct __kfifo *fifo, void __user *to,
 540         unsigned long len, unsigned int *copied, size_t recsize)
 541 {
 542         unsigned long ret;
 543         unsigned int n;
 544
 545         if (fifo->in == fifo->out) {
 546                 *copied = 0;
 547                 return 0;
 548         }
 549
 550         n = __kfifo_peek_n(fifo, recsize);
 551         if (len > n)
 552                 len = n;
 553
 554         ret = kfifo_copy_to_user(fifo, to, len, fifo->out + recsize, copied);
 555         if (unlikely(ret)) {
 556                 *copied = 0;
 557                 return -EFAULT;
 558         }
 559         fifo->out += n + recsize;
 560         return 0;
 561 }
 562 EXPORT_SYMBOL(__kfifo_to_user_r);
 563
 564 unsigned int __kfifo_dma_in_prepare_r(struct __kfifo *fifo,
 565         struct scatterlist *sgl, int nents, unsigned int len, size_t recsize)
 566 {
 567         if (!nents)
 568                 BUG();
 569
 570         len = __kfifo_max_r(len, recsize);
 571
 572         if (len + recsize > kfifo_unused(fifo))
 573                 return 0;
 574
 575         return setup_sgl(fifo, sgl, nents, len, fifo->in + recsize);
 576 }
 577 EXPORT_SYMBOL(__kfifo_dma_in_prepare_r);
 578
 579 void __kfifo_dma_in_finish_r(struct __kfifo *fifo,
 580         unsigned int len, size_t recsize)
 581 {
 582         len = __kfifo_max_r(len, recsize);
 583         __kfifo_poke_n(fifo, len, recsize);
 584         fifo->in += len + recsize;
 585 }
 586 EXPORT_SYMBOL(__kfifo_dma_in_finish_r);
 587
 588 unsigned int __kfifo_dma_out_prepare_r(struct __kfifo *fifo,
 589         struct scatterlist *sgl, int nents, unsigned int len, size_t recsize)
 590 {
 591         if (!nents)
 592                 BUG();
 593
 594         len = __kfifo_max_r(len, recsize);
 595
 596         if (len + recsize > fifo->in - fifo->out)
 597                 return 0;
 598
 599         return setup_sgl(fifo, sgl, nents, len, fifo->out + recsize);
 600 }
 601 EXPORT_SYMBOL(__kfifo_dma_out_prepare_r);
 602
 603 void __kfifo_dma_out_finish_r(struct __kfifo *fifo, size_t recsize)
 604 {
 605         unsigned int len;
 606
 607         len = __kfifo_peek_n(fifo, recsize);
 608         fifo->out += len + recsize;
 609 }
 610 EXPORT_SYMBOL(__kfifo_dma_out_finish_r);