Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 1 | /* |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 2 | * A generic kernel FIFO implementation |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 3 | * |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 4 | * Copyright (C) 2009/2010 Stefani Seibold <stefani@seibold.net> |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 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 |
Stefani Seibold | d78a3ed | 2010-08-11 14:17:27 -0700 | [diff] [blame] | 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 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> |
vignesh babu | f84d5a7 | 2007-07-15 23:41:34 -0700 | [diff] [blame] | 26 | #include <linux/log2.h> |
Stefani Seibold | a121f24 | 2009-12-21 14:37:31 -0800 | [diff] [blame] | 27 | #include <linux/uaccess.h> |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 28 | #include <linux/kfifo.h> |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 29 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 30 | /* |
| 31 | * internal helper to calculate the unused elements in a fifo |
| 32 | */ |
| 33 | static inline unsigned int kfifo_unused(struct __kfifo *fifo) |
Stefani Seibold | 4546548 | 2009-12-21 14:37:26 -0800 | [diff] [blame] | 34 | { |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 35 | return (fifo->mask + 1) - (fifo->in - fifo->out); |
Stefani Seibold | 4546548 | 2009-12-21 14:37:26 -0800 | [diff] [blame] | 36 | } |
| 37 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 38 | int __kfifo_alloc(struct __kfifo *fifo, unsigned int size, |
| 39 | size_t esize, gfp_t gfp_mask) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 40 | { |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 41 | /* |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 42 | * round down to the next power of 2, since our 'let the indices |
Robert P. J. Day | b33112d | 2009-06-16 15:33:34 -0700 | [diff] [blame] | 43 | * wrap' technique works only in this case. |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 44 | */ |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 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; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 56 | } |
| 57 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 58 | fifo->data = kmalloc(size * esize, gfp_mask); |
| 59 | |
| 60 | if (!fifo->data) { |
| 61 | fifo->mask = 0; |
Stefani Seibold | 4546548 | 2009-12-21 14:37:26 -0800 | [diff] [blame] | 62 | return -ENOMEM; |
| 63 | } |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 64 | fifo->mask = size - 1; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 65 | |
Stefani Seibold | 4546548 | 2009-12-21 14:37:26 -0800 | [diff] [blame] | 66 | return 0; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 67 | } |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 68 | EXPORT_SYMBOL(__kfifo_alloc); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 69 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 70 | void __kfifo_free(struct __kfifo *fifo) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 71 | { |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 72 | kfree(fifo->data); |
| 73 | fifo->in = 0; |
| 74 | fifo->out = 0; |
| 75 | fifo->esize = 0; |
| 76 | fifo->data = NULL; |
| 77 | fifo->mask = 0; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 78 | } |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 79 | EXPORT_SYMBOL(__kfifo_free); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 80 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 81 | int __kfifo_init(struct __kfifo *fifo, void *buffer, |
| 82 | unsigned int size, size_t esize) |
Stefani Seibold | a121f24 | 2009-12-21 14:37:31 -0800 | [diff] [blame] | 83 | { |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 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; |
Stefani Seibold | a121f24 | 2009-12-21 14:37:31 -0800 | [diff] [blame] | 97 | } |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 98 | fifo->mask = size - 1; |
Stefani Seibold | a121f24 | 2009-12-21 14:37:31 -0800 | [diff] [blame] | 99 | |
Andi Kleen | 64ce103 | 2010-01-15 17:01:15 -0800 | [diff] [blame] | 100 | return 0; |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 101 | } |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 102 | EXPORT_SYMBOL(__kfifo_init); |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 103 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 104 | static void kfifo_copy_in(struct __kfifo *fifo, const void *src, |
| 105 | unsigned int len, unsigned int off) |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 106 | { |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 107 | unsigned int size = fifo->mask + 1; |
| 108 | unsigned int esize = fifo->esize; |
| 109 | unsigned int l; |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 110 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 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(); |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 126 | } |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 127 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 128 | unsigned int __kfifo_in(struct __kfifo *fifo, |
| 129 | const void *buf, unsigned int len) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 130 | { |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 131 | unsigned int l; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 132 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 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; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 139 | return len; |
| 140 | } |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 141 | EXPORT_SYMBOL(__kfifo_in); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 142 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 143 | static void kfifo_copy_out(struct __kfifo *fifo, void *dst, |
| 144 | unsigned int len, unsigned int off) |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 145 | { |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 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(); |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 165 | } |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 166 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 167 | unsigned int __kfifo_out_peek(struct __kfifo *fifo, |
| 168 | void *buf, unsigned int len) |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 169 | { |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 170 | unsigned int l; |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 171 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 172 | l = fifo->in - fifo->out; |
| 173 | if (len > l) |
| 174 | len = l; |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 175 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 176 | kfifo_copy_out(fifo, buf, len, fifo->out); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 177 | return len; |
| 178 | } |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 179 | EXPORT_SYMBOL(__kfifo_out_peek); |
Stefani Seibold | a121f24 | 2009-12-21 14:37:31 -0800 | [diff] [blame] | 180 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 181 | unsigned int __kfifo_out(struct __kfifo *fifo, |
| 182 | void *buf, unsigned int len) |
Andi Kleen | a5b9e2c | 2010-01-15 17:01:16 -0800 | [diff] [blame] | 183 | { |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 184 | len = __kfifo_out_peek(fifo, buf, len); |
| 185 | fifo->out += len; |
Andi Kleen | a5b9e2c | 2010-01-15 17:01:16 -0800 | [diff] [blame] | 186 | return len; |
| 187 | } |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 188 | EXPORT_SYMBOL(__kfifo_out); |
Andi Kleen | a5b9e2c | 2010-01-15 17:01:16 -0800 | [diff] [blame] | 189 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 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) |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 193 | { |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 194 | unsigned int size = fifo->mask + 1; |
| 195 | unsigned int esize = fifo->esize; |
| 196 | unsigned int l; |
| 197 | unsigned long ret; |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 198 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 199 | off &= fifo->mask; |
| 200 | if (esize != 1) { |
| 201 | off *= esize; |
| 202 | size *= esize; |
| 203 | len *= esize; |
| 204 | } |
| 205 | l = min(len, size - off); |
Andi Kleen | 64ce103 | 2010-01-15 17:01:15 -0800 | [diff] [blame] | 206 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 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 */ |
Andi Kleen | 64ce103 | 2010-01-15 17:01:15 -0800 | [diff] [blame] | 222 | return ret; |
Stefani Seibold | a121f24 | 2009-12-21 14:37:31 -0800 | [diff] [blame] | 223 | } |
Stefani Seibold | a121f24 | 2009-12-21 14:37:31 -0800 | [diff] [blame] | 224 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 225 | int __kfifo_from_user(struct __kfifo *fifo, const void __user *from, |
| 226 | unsigned long len, unsigned int *copied) |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 227 | { |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 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; |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 248 | } |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 249 | EXPORT_SYMBOL(__kfifo_from_user); |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 250 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 251 | static unsigned long kfifo_copy_to_user(struct __kfifo *fifo, void __user *to, |
| 252 | unsigned int len, unsigned int off, unsigned int *copied) |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 253 | { |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 254 | unsigned int l; |
| 255 | unsigned long ret; |
| 256 | unsigned int size = fifo->mask + 1; |
| 257 | unsigned int esize = fifo->esize; |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 258 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 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(__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) { |
Stefani Seibold | d78a3ed | 2010-08-11 14:17:27 -0700 | [diff] [blame] | 336 | sg_set_page(sgl, page, l - off, off); |
| 337 | sgl = sg_next(sgl); |
| 338 | if (++n == nents || sgl == NULL) |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 339 | return n; |
| 340 | page = npage; |
| 341 | len -= l - off; |
| 342 | l = off = 0; |
| 343 | } |
| 344 | } |
Stefani Seibold | d78a3ed | 2010-08-11 14:17:27 -0700 | [diff] [blame] | 345 | sg_set_page(sgl, page, len, off); |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 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 | |
Stefani Seibold | d78a3ed | 2010-08-11 14:17:27 -0700 | [diff] [blame] | 365 | n = setup_sgl_buf(sgl, fifo->data + off, nents, l); |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 366 | n += setup_sgl_buf(sgl + n, fifo->data, nents - n, len - l); |
| 367 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 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(__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(__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 | { |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 448 | return __kfifo_peek_n(fifo, recsize); |
| 449 | } |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 450 | EXPORT_SYMBOL(__kfifo_len_r); |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 451 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 452 | unsigned int __kfifo_in_r(struct __kfifo *fifo, const void *buf, |
| 453 | unsigned int len, size_t recsize) |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 454 | { |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 455 | if (len + recsize > kfifo_unused(fifo)) |
| 456 | return 0; |
Stefani Seibold | 86d4880 | 2009-12-21 14:37:32 -0800 | [diff] [blame] | 457 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 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(__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(__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(__kfifo_out_r); |
| 503 | |
Andrea Righi | b35de43 | 2010-08-19 14:13:27 -0700 | [diff] [blame] | 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(__kfifo_skip_r); |
| 512 | |
Stefani Seibold | 2e956fb | 2010-08-10 18:03:38 -0700 | [diff] [blame] | 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(__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(__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(__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(__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(__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(__kfifo_dma_out_finish_r); |