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 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(size * esize, 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 } 67 EXPORT_SYMBOL(__kfifo_alloc); 68 69 void __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 } 78 EXPORT_SYMBOL(__kfifo_free); 79 80 int __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 } 100 EXPORT_SYMBOL(__kfifo_init); 101 102 static 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 126 unsigned 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 } 139 EXPORT_SYMBOL(__kfifo_in); 140 141 static 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 165 unsigned 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 } 177 EXPORT_SYMBOL(__kfifo_out_peek); 178 179 unsigned 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 } 186 EXPORT_SYMBOL(__kfifo_out); 187 188 static 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 223 int __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 } 247 EXPORT_SYMBOL(__kfifo_from_user); 248 249 static 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 283 int __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 } 306 EXPORT_SYMBOL(__kfifo_to_user); 307 308 static 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 347 static 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 369 unsigned 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 } 380 EXPORT_SYMBOL(__kfifo_dma_in_prepare); 381 382 unsigned 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 } 393 EXPORT_SYMBOL(__kfifo_dma_out_prepare); 394 395 unsigned 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 } 403 EXPORT_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 */ 411 static 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 */ 434 static 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 445 unsigned int __kfifo_len_r(struct __kfifo *fifo, size_t recsize) 446 { 447 return __kfifo_peek_n(fifo, recsize); 448 } 449 EXPORT_SYMBOL(__kfifo_len_r); 450 451 unsigned 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 } 463 EXPORT_SYMBOL(__kfifo_in_r); 464 465 static 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 477 unsigned 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 } 487 EXPORT_SYMBOL(__kfifo_out_peek_r); 488 489 unsigned 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 } 501 EXPORT_SYMBOL(__kfifo_out_r); 502 503 void __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 } 510 EXPORT_SYMBOL(__kfifo_skip_r); 511 512 int __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 } 534 EXPORT_SYMBOL(__kfifo_from_user_r); 535 536 int __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 } 559 EXPORT_SYMBOL(__kfifo_to_user_r); 560 561 unsigned int __kfifo_dma_in_prepare_r(struct __kfifo *fifo, 562 struct scatterlist *sgl, int nents, unsigned int len, size_t recsize) 563 { 564 if (!nents) 565 BUG(); 566 567 len = __kfifo_max_r(len, recsize); 568 569 if (len + recsize > kfifo_unused(fifo)) 570 return 0; 571 572 return setup_sgl(fifo, sgl, nents, len, fifo->in + recsize); 573 } 574 EXPORT_SYMBOL(__kfifo_dma_in_prepare_r); 575 576 void __kfifo_dma_in_finish_r(struct __kfifo *fifo, 577 unsigned int len, size_t recsize) 578 { 579 len = __kfifo_max_r(len, recsize); 580 __kfifo_poke_n(fifo, len, recsize); 581 fifo->in += len + recsize; 582 } 583 EXPORT_SYMBOL(__kfifo_dma_in_finish_r); 584 585 unsigned int __kfifo_dma_out_prepare_r(struct __kfifo *fifo, 586 struct scatterlist *sgl, int nents, unsigned int len, size_t recsize) 587 { 588 if (!nents) 589 BUG(); 590 591 len = __kfifo_max_r(len, recsize); 592 593 if (len + recsize > fifo->in - fifo->out) 594 return 0; 595 596 return setup_sgl(fifo, sgl, nents, len, fifo->out + recsize); 597 } 598 EXPORT_SYMBOL(__kfifo_dma_out_prepare_r); 599 600 void __kfifo_dma_out_finish_r(struct __kfifo *fifo, size_t recsize) 601 { 602 unsigned int len; 603 604 len = __kfifo_peek_n(fifo, recsize); 605 fifo->out += len + recsize; 606 } 607 EXPORT_SYMBOL(__kfifo_dma_out_finish_r); 608