Back to home page

LXR

 
 

    


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