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