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