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