← Back to branch summary

~ubuntu-branches/ubuntu/quantal/linux-backports-modules-3.5.0/quantal-updates

« back to all changes in this revision

Viewing changes to updates/cw-3.6/compat/kfifo.c

  • Committer: Package Import Robot
  • Author(s): Leann Ogasawara
  • Date: 2012-10-10 22:28:55 UTC
  • Revision ID: package-import@ubuntu.com-20121010222855-qepocc61xktv6gs9
Tags: 3.5.0-17.1
https://launchpad.net/bugs/1066123
* Open Quantal LBM
* Add compat-wireless 3.6
  -LP: #1066123

Show diffs side-by-side

added added

removed removed

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