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