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