the-tree/lib/kfifo.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * A generic kernel FIFO implementation
0004  *
0005  * Copyright (C) 2009/2010 Stefani Seibold <stefani@seibold.net>
0006  */
0007
0008 #include <linux/kernel.h>
0009 #include <linux/export.h>
0010 #include <linux/slab.h>
0011 #include <linux/err.h>
0012 #include <linux/log2.h>
0013 #include <linux/uaccess.h>
0014 #include <linux/kfifo.h>
0015
0016 /*
0017  * internal helper to calculate the unused elements in a fifo
0018  */
0019 static inline unsigned int kfifo_unused(struct __kfifo *fifo)
0020 {
0021     return (fifo->mask + 1) - (fifo->in - fifo->out);
0022 }
0023
0024 int __kfifo_alloc(struct __kfifo *fifo, unsigned int size,
0025         size_t esize, gfp_t gfp_mask)
0026 {
0027     /*
0028      * round up to the next power of 2, since our 'let the indices
0029      * wrap' technique works only in this case.
0030      */
0031     size = roundup_pow_of_two(size);
0032
0033     fifo->in = 0;
0034     fifo->out = 0;
0035     fifo->esize = esize;
0036
0037     if (size < 2) {
0038         fifo->data = NULL;
0039         fifo->mask = 0;
0040         return -EINVAL;
0041     }
0042
0043     fifo->data = kmalloc_array(esize, size, gfp_mask);
0044
0045     if (!fifo->data) {
0046         fifo->mask = 0;
0047         return -ENOMEM;
0048     }
0049     fifo->mask = size - 1;
0050
0051     return 0;
0052 }
0053 EXPORT_SYMBOL(__kfifo_alloc);
0054
0055 void __kfifo_free(struct __kfifo *fifo)
0056 {
0057     kfree(fifo->data);
0058     fifo->in = 0;
0059     fifo->out = 0;
0060     fifo->esize = 0;
0061     fifo->data = NULL;
0062     fifo->mask = 0;
0063 }
0064 EXPORT_SYMBOL(__kfifo_free);
0065
0066 int __kfifo_init(struct __kfifo *fifo, void *buffer,
0067         unsigned int size, size_t esize)
0068 {
0069     size /= esize;
0070
0071     if (!is_power_of_2(size))
0072         size = rounddown_pow_of_two(size);
0073
0074     fifo->in = 0;
0075     fifo->out = 0;
0076     fifo->esize = esize;
0077     fifo->data = buffer;
0078
0079     if (size < 2) {
0080         fifo->mask = 0;
0081         return -EINVAL;
0082     }
0083     fifo->mask = size - 1;
0084
0085     return 0;
0086 }
0087 EXPORT_SYMBOL(__kfifo_init);
0088
0089 static void kfifo_copy_in(struct __kfifo *fifo, const void *src,
0090         unsigned int len, unsigned int off)
0091 {
0092     unsigned int size = fifo->mask + 1;
0093     unsigned int esize = fifo->esize;
0094     unsigned int l;
0095
0096     off &= fifo->mask;
0097     if (esize != 1) {
0098         off *= esize;
0099         size *= esize;
0100         len *= esize;
0101     }
0102     l = min(len, size - off);
0103
0104     memcpy(fifo->data + off, src, l);
0105     memcpy(fifo->data, src + l, len - l);
0106     /*
0107      * make sure that the data in the fifo is up to date before
0108      * incrementing the fifo->in index counter
0109      */
0110     smp_wmb();
0111 }
0112
0113 unsigned int __kfifo_in(struct __kfifo *fifo,
0114         const void *buf, unsigned int len)
0115 {
0116     unsigned int l;
0117
0118     l = kfifo_unused(fifo);
0119     if (len > l)
0120         len = l;
0121
0122     kfifo_copy_in(fifo, buf, len, fifo->in);
0123     fifo->in += len;
0124     return len;
0125 }
0126 EXPORT_SYMBOL(__kfifo_in);
0127
0128 static void kfifo_copy_out(struct __kfifo *fifo, void *dst,
0129         unsigned int len, unsigned int off)
0130 {
0131     unsigned int size = fifo->mask + 1;
0132     unsigned int esize = fifo->esize;
0133     unsigned int l;
0134
0135     off &= fifo->mask;
0136     if (esize != 1) {
0137         off *= esize;
0138         size *= esize;
0139         len *= esize;
0140     }
0141     l = min(len, size - off);
0142
0143     memcpy(dst, fifo->data + off, l);
0144     memcpy(dst + l, fifo->data, len - l);
0145     /*
0146      * make sure that the data is copied before
0147      * incrementing the fifo->out index counter
0148      */
0149     smp_wmb();
0150 }
0151
0152 unsigned int __kfifo_out_peek(struct __kfifo *fifo,
0153         void *buf, unsigned int len)
0154 {
0155     unsigned int l;
0156
0157     l = fifo->in - fifo->out;
0158     if (len > l)
0159         len = l;
0160
0161     kfifo_copy_out(fifo, buf, len, fifo->out);
0162     return len;
0163 }
0164 EXPORT_SYMBOL(__kfifo_out_peek);
0165
0166 unsigned int __kfifo_out(struct __kfifo *fifo,
0167         void *buf, unsigned int len)
0168 {
0169     len = __kfifo_out_peek(fifo, buf, len);
0170     fifo->out += len;
0171     return len;
0172 }
0173 EXPORT_SYMBOL(__kfifo_out);
0174
0175 static unsigned long kfifo_copy_from_user(struct __kfifo *fifo,
0176     const void __user *from, unsigned int len, unsigned int off,
0177     unsigned int *copied)
0178 {
0179     unsigned int size = fifo->mask + 1;
0180     unsigned int esize = fifo->esize;
0181     unsigned int l;
0182     unsigned long ret;
0183
0184     off &= fifo->mask;
0185     if (esize != 1) {
0186         off *= esize;
0187         size *= esize;
0188         len *= esize;
0189     }
0190     l = min(len, size - off);
0191
0192     ret = copy_from_user(fifo->data + off, from, l);
0193     if (unlikely(ret))
0194         ret = DIV_ROUND_UP(ret + len - l, esize);
0195     else {
0196         ret = copy_from_user(fifo->data, from + l, len - l);
0197         if (unlikely(ret))
0198             ret = DIV_ROUND_UP(ret, esize);
0199     }
0200     /*
0201      * make sure that the data in the fifo is up to date before
0202      * incrementing the fifo->in index counter
0203      */
0204     smp_wmb();
0205     *copied = len - ret * esize;
0206     /* return the number of elements which are not copied */
0207     return ret;
0208 }
0209
0210 int __kfifo_from_user(struct __kfifo *fifo, const void __user *from,
0211         unsigned long len, unsigned int *copied)
0212 {
0213     unsigned int l;
0214     unsigned long ret;
0215     unsigned int esize = fifo->esize;
0216     int err;
0217
0218     if (esize != 1)
0219         len /= esize;
0220
0221     l = kfifo_unused(fifo);
0222     if (len > l)
0223         len = l;
0224
0225     ret = kfifo_copy_from_user(fifo, from, len, fifo->in, copied);
0226     if (unlikely(ret)) {
0227         len -= ret;
0228         err = -EFAULT;
0229     } else
0230         err = 0;
0231     fifo->in += len;
0232     return err;
0233 }
0234 EXPORT_SYMBOL(__kfifo_from_user);
0235
0236 static unsigned long kfifo_copy_to_user(struct __kfifo *fifo, void __user *to,
0237         unsigned int len, unsigned int off, unsigned int *copied)
0238 {
0239     unsigned int l;
0240     unsigned long ret;
0241     unsigned int size = fifo->mask + 1;
0242     unsigned int esize = fifo->esize;
0243
0244     off &= fifo->mask;
0245     if (esize != 1) {
0246         off *= esize;
0247         size *= esize;
0248         len *= esize;
0249     }
0250     l = min(len, size - off);
0251
0252     ret = copy_to_user(to, fifo->data + off, l);
0253     if (unlikely(ret))
0254         ret = DIV_ROUND_UP(ret + len - l, esize);
0255     else {
0256         ret = copy_to_user(to + l, fifo->data, len - l);
0257         if (unlikely(ret))
0258             ret = DIV_ROUND_UP(ret, esize);
0259     }
0260     /*
0261      * make sure that the data is copied before
0262      * incrementing the fifo->out index counter
0263      */
0264     smp_wmb();
0265     *copied = len - ret * esize;
0266     /* return the number of elements which are not copied */
0267     return ret;
0268 }
0269
0270 int __kfifo_to_user(struct __kfifo *fifo, void __user *to,
0271         unsigned long len, unsigned int *copied)
0272 {
0273     unsigned int l;
0274     unsigned long ret;
0275     unsigned int esize = fifo->esize;
0276     int err;
0277
0278     if (esize != 1)
0279         len /= esize;
0280
0281     l = fifo->in - fifo->out;
0282     if (len > l)
0283         len = l;
0284     ret = kfifo_copy_to_user(fifo, to, len, fifo->out, copied);
0285     if (unlikely(ret)) {
0286         len -= ret;
0287         err = -EFAULT;
0288     } else
0289         err = 0;
0290     fifo->out += len;
0291     return err;
0292 }
0293 EXPORT_SYMBOL(__kfifo_to_user);
0294
0295 static int setup_sgl_buf(struct scatterlist *sgl, void *buf,
0296         int nents, unsigned int len)
0297 {
0298     int n;
0299     unsigned int l;
0300     unsigned int off;
0301     struct page *page;
0302
0303     if (!nents)
0304         return 0;
0305
0306     if (!len)
0307         return 0;
0308
0309     n = 0;
0310     page = virt_to_page(buf);
0311     off = offset_in_page(buf);
0312     l = 0;
0313
0314     while (len >= l + PAGE_SIZE - off) {
0315         struct page *npage;
0316
0317         l += PAGE_SIZE;
0318         buf += PAGE_SIZE;
0319         npage = virt_to_page(buf);
0320         if (page_to_phys(page) != page_to_phys(npage) - l) {
0321             sg_set_page(sgl, page, l - off, off);
0322             sgl = sg_next(sgl);
0323             if (++n == nents || sgl == NULL)
0324                 return n;
0325             page = npage;
0326             len -= l - off;
0327             l = off = 0;
0328         }
0329     }
0330     sg_set_page(sgl, page, len, off);
0331     return n + 1;
0332 }
0333
0334 static unsigned int setup_sgl(struct __kfifo *fifo, struct scatterlist *sgl,
0335         int nents, unsigned int len, unsigned int off)
0336 {
0337     unsigned int size = fifo->mask + 1;
0338     unsigned int esize = fifo->esize;
0339     unsigned int l;
0340     unsigned int n;
0341
0342     off &= fifo->mask;
0343     if (esize != 1) {
0344         off *= esize;
0345         size *= esize;
0346         len *= esize;
0347     }
0348     l = min(len, size - off);
0349
0350     n = setup_sgl_buf(sgl, fifo->data + off, nents, l);
0351     n += setup_sgl_buf(sgl + n, fifo->data, nents - n, len - l);
0352
0353     return n;
0354 }
0355
0356 unsigned int __kfifo_dma_in_prepare(struct __kfifo *fifo,
0357         struct scatterlist *sgl, int nents, unsigned int len)
0358 {
0359     unsigned int l;
0360
0361     l = kfifo_unused(fifo);
0362     if (len > l)
0363         len = l;
0364
0365     return setup_sgl(fifo, sgl, nents, len, fifo->in);
0366 }
0367 EXPORT_SYMBOL(__kfifo_dma_in_prepare);
0368
0369 unsigned int __kfifo_dma_out_prepare(struct __kfifo *fifo,
0370         struct scatterlist *sgl, int nents, unsigned int len)
0371 {
0372     unsigned int l;
0373
0374     l = fifo->in - fifo->out;
0375     if (len > l)
0376         len = l;
0377
0378     return setup_sgl(fifo, sgl, nents, len, fifo->out);
0379 }
0380 EXPORT_SYMBOL(__kfifo_dma_out_prepare);
0381
0382 unsigned int __kfifo_max_r(unsigned int len, size_t recsize)
0383 {
0384     unsigned int max = (1 << (recsize << 3)) - 1;
0385
0386     if (len > max)
0387         return max;
0388     return len;
0389 }
0390 EXPORT_SYMBOL(__kfifo_max_r);
0391
0392 #define __KFIFO_PEEK(data, out, mask) \
0393     ((data)[(out) & (mask)])
0394 /*
0395  * __kfifo_peek_n internal helper function for determinate the length of
0396  * the next record in the fifo
0397  */
0398 static unsigned int __kfifo_peek_n(struct __kfifo *fifo, size_t recsize)
0399 {
0400     unsigned int l;
0401     unsigned int mask = fifo->mask;
0402     unsigned char *data = fifo->data;
0403
0404     l = __KFIFO_PEEK(data, fifo->out, mask);
0405
0406     if (--recsize)
0407         l |= __KFIFO_PEEK(data, fifo->out + 1, mask) << 8;
0408
0409     return l;
0410 }
0411
0412 #define __KFIFO_POKE(data, in, mask, val) \
0413     ( \
0414     (data)[(in) & (mask)] = (unsigned char)(val) \
0415     )
0416
0417 /*
0418  * __kfifo_poke_n internal helper function for storing the length of
0419  * the record into the fifo
0420  */
0421 static void __kfifo_poke_n(struct __kfifo *fifo, unsigned int n, size_t recsize)
0422 {
0423     unsigned int mask = fifo->mask;
0424     unsigned char *data = fifo->data;
0425
0426     __KFIFO_POKE(data, fifo->in, mask, n);
0427
0428     if (recsize > 1)
0429         __KFIFO_POKE(data, fifo->in + 1, mask, n >> 8);
0430 }
0431
0432 unsigned int __kfifo_len_r(struct __kfifo *fifo, size_t recsize)
0433 {
0434     return __kfifo_peek_n(fifo, recsize);
0435 }
0436 EXPORT_SYMBOL(__kfifo_len_r);
0437
0438 unsigned int __kfifo_in_r(struct __kfifo *fifo, const void *buf,
0439         unsigned int len, size_t recsize)
0440 {
0441     if (len + recsize > kfifo_unused(fifo))
0442         return 0;
0443
0444     __kfifo_poke_n(fifo, len, recsize);
0445
0446     kfifo_copy_in(fifo, buf, len, fifo->in + recsize);
0447     fifo->in += len + recsize;
0448     return len;
0449 }
0450 EXPORT_SYMBOL(__kfifo_in_r);
0451
0452 static unsigned int kfifo_out_copy_r(struct __kfifo *fifo,
0453     void *buf, unsigned int len, size_t recsize, unsigned int *n)
0454 {
0455     *n = __kfifo_peek_n(fifo, recsize);
0456
0457     if (len > *n)
0458         len = *n;
0459
0460     kfifo_copy_out(fifo, buf, len, fifo->out + recsize);
0461     return len;
0462 }
0463
0464 unsigned int __kfifo_out_peek_r(struct __kfifo *fifo, void *buf,
0465         unsigned int len, size_t recsize)
0466 {
0467     unsigned int n;
0468
0469     if (fifo->in == fifo->out)
0470         return 0;
0471
0472     return kfifo_out_copy_r(fifo, buf, len, recsize, &n);
0473 }
0474 EXPORT_SYMBOL(__kfifo_out_peek_r);
0475
0476 unsigned int __kfifo_out_r(struct __kfifo *fifo, void *buf,
0477         unsigned int len, size_t recsize)
0478 {
0479     unsigned int n;
0480
0481     if (fifo->in == fifo->out)
0482         return 0;
0483
0484     len = kfifo_out_copy_r(fifo, buf, len, recsize, &n);
0485     fifo->out += n + recsize;
0486     return len;
0487 }
0488 EXPORT_SYMBOL(__kfifo_out_r);
0489
0490 void __kfifo_skip_r(struct __kfifo *fifo, size_t recsize)
0491 {
0492     unsigned int n;
0493
0494     n = __kfifo_peek_n(fifo, recsize);
0495     fifo->out += n + recsize;
0496 }
0497 EXPORT_SYMBOL(__kfifo_skip_r);
0498
0499 int __kfifo_from_user_r(struct __kfifo *fifo, const void __user *from,
0500     unsigned long len, unsigned int *copied, size_t recsize)
0501 {
0502     unsigned long ret;
0503
0504     len = __kfifo_max_r(len, recsize);
0505
0506     if (len + recsize > kfifo_unused(fifo)) {
0507         *copied = 0;
0508         return 0;
0509     }
0510
0511     __kfifo_poke_n(fifo, len, recsize);
0512
0513     ret = kfifo_copy_from_user(fifo, from, len, fifo->in + recsize, copied);
0514     if (unlikely(ret)) {
0515         *copied = 0;
0516         return -EFAULT;
0517     }
0518     fifo->in += len + recsize;
0519     return 0;
0520 }
0521 EXPORT_SYMBOL(__kfifo_from_user_r);
0522
0523 int __kfifo_to_user_r(struct __kfifo *fifo, void __user *to,
0524     unsigned long len, unsigned int *copied, size_t recsize)
0525 {
0526     unsigned long ret;
0527     unsigned int n;
0528
0529     if (fifo->in == fifo->out) {
0530         *copied = 0;
0531         return 0;
0532     }
0533
0534     n = __kfifo_peek_n(fifo, recsize);
0535     if (len > n)
0536         len = n;
0537
0538     ret = kfifo_copy_to_user(fifo, to, len, fifo->out + recsize, copied);
0539     if (unlikely(ret)) {
0540         *copied = 0;
0541         return -EFAULT;
0542     }
0543     fifo->out += n + recsize;
0544     return 0;
0545 }
0546 EXPORT_SYMBOL(__kfifo_to_user_r);
0547
0548 unsigned int __kfifo_dma_in_prepare_r(struct __kfifo *fifo,
0549     struct scatterlist *sgl, int nents, unsigned int len, size_t recsize)
0550 {
0551     BUG_ON(!nents);
0552
0553     len = __kfifo_max_r(len, recsize);
0554
0555     if (len + recsize > kfifo_unused(fifo))
0556         return 0;
0557
0558     return setup_sgl(fifo, sgl, nents, len, fifo->in + recsize);
0559 }
0560 EXPORT_SYMBOL(__kfifo_dma_in_prepare_r);
0561
0562 void __kfifo_dma_in_finish_r(struct __kfifo *fifo,
0563     unsigned int len, size_t recsize)
0564 {
0565     len = __kfifo_max_r(len, recsize);
0566     __kfifo_poke_n(fifo, len, recsize);
0567     fifo->in += len + recsize;
0568 }
0569 EXPORT_SYMBOL(__kfifo_dma_in_finish_r);
0570
0571 unsigned int __kfifo_dma_out_prepare_r(struct __kfifo *fifo,
0572     struct scatterlist *sgl, int nents, unsigned int len, size_t recsize)
0573 {
0574     BUG_ON(!nents);
0575
0576     len = __kfifo_max_r(len, recsize);
0577
0578     if (len + recsize > fifo->in - fifo->out)
0579         return 0;
0580
0581     return setup_sgl(fifo, sgl, nents, len, fifo->out + recsize);
0582 }
0583 EXPORT_SYMBOL(__kfifo_dma_out_prepare_r);
0584
0585 void __kfifo_dma_out_finish_r(struct __kfifo *fifo, size_t recsize)
0586 {
0587     unsigned int len;
0588
0589     len = __kfifo_peek_n(fifo, recsize);
0590     fifo->out += len + recsize;
0591 }
0592 EXPORT_SYMBOL(__kfifo_dma_out_finish_r);