Sjur Braendeland | 15c9ac0 | 2010-03-30 13:56:24 +0000 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (C) ST-Ericsson AB 2010 |
| 3 | * Author: Sjur Brendeland/sjur.brandeland@stericsson.com |
| 4 | * License terms: GNU General Public License (GPL) version 2 |
| 5 | */ |
| 6 | |
| 7 | #include <linux/string.h> |
| 8 | #include <linux/skbuff.h> |
| 9 | #include <linux/hardirq.h> |
| 10 | #include <net/caif/cfpkt.h> |
| 11 | |
Sjur Braendeland | 24e263a | 2010-08-10 07:36:46 +0000 | [diff] [blame] | 12 | #define PKT_PREFIX 48 |
Sjur Braendeland | 2aa40ae | 2010-06-17 06:55:40 +0000 | [diff] [blame] | 13 | #define PKT_POSTFIX 2 |
Sjur Braendeland | 15c9ac0 | 2010-03-30 13:56:24 +0000 | [diff] [blame] | 14 | #define PKT_LEN_WHEN_EXTENDING 128 |
| 15 | #define PKT_ERROR(pkt, errmsg) do { \ |
| 16 | cfpkt_priv(pkt)->erronous = true; \ |
| 17 | skb_reset_tail_pointer(&pkt->skb); \ |
| 18 | pr_warning("CAIF: " errmsg);\ |
| 19 | } while (0) |
| 20 | |
| 21 | struct cfpktq { |
| 22 | struct sk_buff_head head; |
| 23 | atomic_t count; |
| 24 | /* Lock protects count updates */ |
| 25 | spinlock_t lock; |
| 26 | }; |
| 27 | |
| 28 | /* |
| 29 | * net/caif/ is generic and does not |
| 30 | * understand SKB, so we do this typecast |
| 31 | */ |
| 32 | struct cfpkt { |
| 33 | struct sk_buff skb; |
| 34 | }; |
| 35 | |
| 36 | /* Private data inside SKB */ |
| 37 | struct cfpkt_priv_data { |
| 38 | struct dev_info dev_info; |
| 39 | bool erronous; |
| 40 | }; |
| 41 | |
| 42 | inline struct cfpkt_priv_data *cfpkt_priv(struct cfpkt *pkt) |
| 43 | { |
| 44 | return (struct cfpkt_priv_data *) pkt->skb.cb; |
| 45 | } |
| 46 | |
| 47 | inline bool is_erronous(struct cfpkt *pkt) |
| 48 | { |
| 49 | return cfpkt_priv(pkt)->erronous; |
| 50 | } |
| 51 | |
| 52 | inline struct sk_buff *pkt_to_skb(struct cfpkt *pkt) |
| 53 | { |
| 54 | return &pkt->skb; |
| 55 | } |
| 56 | |
| 57 | inline struct cfpkt *skb_to_pkt(struct sk_buff *skb) |
| 58 | { |
| 59 | return (struct cfpkt *) skb; |
| 60 | } |
| 61 | |
| 62 | |
| 63 | struct cfpkt *cfpkt_fromnative(enum caif_direction dir, void *nativepkt) |
| 64 | { |
| 65 | struct cfpkt *pkt = skb_to_pkt(nativepkt); |
| 66 | cfpkt_priv(pkt)->erronous = false; |
| 67 | return pkt; |
| 68 | } |
| 69 | EXPORT_SYMBOL(cfpkt_fromnative); |
| 70 | |
| 71 | void *cfpkt_tonative(struct cfpkt *pkt) |
| 72 | { |
| 73 | return (void *) pkt; |
| 74 | } |
| 75 | EXPORT_SYMBOL(cfpkt_tonative); |
| 76 | |
| 77 | static struct cfpkt *cfpkt_create_pfx(u16 len, u16 pfx) |
| 78 | { |
| 79 | struct sk_buff *skb; |
| 80 | |
| 81 | if (likely(in_interrupt())) |
| 82 | skb = alloc_skb(len + pfx, GFP_ATOMIC); |
| 83 | else |
| 84 | skb = alloc_skb(len + pfx, GFP_KERNEL); |
| 85 | |
| 86 | if (unlikely(skb == NULL)) |
| 87 | return NULL; |
| 88 | |
| 89 | skb_reserve(skb, pfx); |
| 90 | return skb_to_pkt(skb); |
| 91 | } |
| 92 | |
| 93 | inline struct cfpkt *cfpkt_create(u16 len) |
| 94 | { |
| 95 | return cfpkt_create_pfx(len + PKT_POSTFIX, PKT_PREFIX); |
| 96 | } |
| 97 | EXPORT_SYMBOL(cfpkt_create); |
| 98 | |
| 99 | void cfpkt_destroy(struct cfpkt *pkt) |
| 100 | { |
| 101 | struct sk_buff *skb = pkt_to_skb(pkt); |
| 102 | kfree_skb(skb); |
| 103 | } |
| 104 | EXPORT_SYMBOL(cfpkt_destroy); |
| 105 | |
| 106 | inline bool cfpkt_more(struct cfpkt *pkt) |
| 107 | { |
| 108 | struct sk_buff *skb = pkt_to_skb(pkt); |
| 109 | return skb->len > 0; |
| 110 | } |
| 111 | EXPORT_SYMBOL(cfpkt_more); |
| 112 | |
| 113 | int cfpkt_peek_head(struct cfpkt *pkt, void *data, u16 len) |
| 114 | { |
| 115 | struct sk_buff *skb = pkt_to_skb(pkt); |
| 116 | if (skb_headlen(skb) >= len) { |
| 117 | memcpy(data, skb->data, len); |
| 118 | return 0; |
| 119 | } |
| 120 | return !cfpkt_extr_head(pkt, data, len) && |
| 121 | !cfpkt_add_head(pkt, data, len); |
| 122 | } |
| 123 | EXPORT_SYMBOL(cfpkt_peek_head); |
| 124 | |
| 125 | int cfpkt_extr_head(struct cfpkt *pkt, void *data, u16 len) |
| 126 | { |
| 127 | struct sk_buff *skb = pkt_to_skb(pkt); |
| 128 | u8 *from; |
| 129 | if (unlikely(is_erronous(pkt))) |
| 130 | return -EPROTO; |
| 131 | |
| 132 | if (unlikely(len > skb->len)) { |
| 133 | PKT_ERROR(pkt, "cfpkt_extr_head read beyond end of packet\n"); |
| 134 | return -EPROTO; |
| 135 | } |
| 136 | |
| 137 | if (unlikely(len > skb_headlen(skb))) { |
| 138 | if (unlikely(skb_linearize(skb) != 0)) { |
| 139 | PKT_ERROR(pkt, "cfpkt_extr_head linearize failed\n"); |
| 140 | return -EPROTO; |
| 141 | } |
| 142 | } |
| 143 | from = skb_pull(skb, len); |
| 144 | from -= len; |
| 145 | memcpy(data, from, len); |
| 146 | return 0; |
| 147 | } |
| 148 | EXPORT_SYMBOL(cfpkt_extr_head); |
| 149 | |
| 150 | int cfpkt_extr_trail(struct cfpkt *pkt, void *dta, u16 len) |
| 151 | { |
| 152 | struct sk_buff *skb = pkt_to_skb(pkt); |
| 153 | u8 *data = dta; |
| 154 | u8 *from; |
| 155 | if (unlikely(is_erronous(pkt))) |
| 156 | return -EPROTO; |
| 157 | |
| 158 | if (unlikely(skb_linearize(skb) != 0)) { |
| 159 | PKT_ERROR(pkt, "cfpkt_extr_trail linearize failed\n"); |
| 160 | return -EPROTO; |
| 161 | } |
| 162 | if (unlikely(skb->data + len > skb_tail_pointer(skb))) { |
| 163 | PKT_ERROR(pkt, "cfpkt_extr_trail read beyond end of packet\n"); |
| 164 | return -EPROTO; |
| 165 | } |
| 166 | from = skb_tail_pointer(skb) - len; |
| 167 | skb_trim(skb, skb->len - len); |
| 168 | memcpy(data, from, len); |
| 169 | return 0; |
| 170 | } |
| 171 | EXPORT_SYMBOL(cfpkt_extr_trail); |
| 172 | |
| 173 | int cfpkt_pad_trail(struct cfpkt *pkt, u16 len) |
| 174 | { |
| 175 | return cfpkt_add_body(pkt, NULL, len); |
| 176 | } |
| 177 | EXPORT_SYMBOL(cfpkt_pad_trail); |
| 178 | |
| 179 | int cfpkt_add_body(struct cfpkt *pkt, const void *data, u16 len) |
| 180 | { |
| 181 | struct sk_buff *skb = pkt_to_skb(pkt); |
| 182 | struct sk_buff *lastskb; |
| 183 | u8 *to; |
| 184 | u16 addlen = 0; |
| 185 | |
| 186 | |
| 187 | if (unlikely(is_erronous(pkt))) |
| 188 | return -EPROTO; |
| 189 | |
| 190 | lastskb = skb; |
| 191 | |
| 192 | /* Check whether we need to add space at the tail */ |
| 193 | if (unlikely(skb_tailroom(skb) < len)) { |
| 194 | if (likely(len < PKT_LEN_WHEN_EXTENDING)) |
| 195 | addlen = PKT_LEN_WHEN_EXTENDING; |
| 196 | else |
| 197 | addlen = len; |
| 198 | } |
| 199 | |
| 200 | /* Check whether we need to change the SKB before writing to the tail */ |
| 201 | if (unlikely((addlen > 0) || skb_cloned(skb) || skb_shared(skb))) { |
| 202 | |
| 203 | /* Make sure data is writable */ |
| 204 | if (unlikely(skb_cow_data(skb, addlen, &lastskb) < 0)) { |
| 205 | PKT_ERROR(pkt, "cfpkt_add_body: cow failed\n"); |
| 206 | return -EPROTO; |
| 207 | } |
| 208 | /* |
| 209 | * Is the SKB non-linear after skb_cow_data()? If so, we are |
| 210 | * going to add data to the last SKB, so we need to adjust |
| 211 | * lengths of the top SKB. |
| 212 | */ |
| 213 | if (lastskb != skb) { |
| 214 | pr_warning("CAIF: %s(): Packet is non-linear\n", |
| 215 | __func__); |
| 216 | skb->len += len; |
| 217 | skb->data_len += len; |
| 218 | } |
| 219 | } |
| 220 | |
| 221 | /* All set to put the last SKB and optionally write data there. */ |
| 222 | to = skb_put(lastskb, len); |
| 223 | if (likely(data)) |
| 224 | memcpy(to, data, len); |
| 225 | return 0; |
| 226 | } |
| 227 | EXPORT_SYMBOL(cfpkt_add_body); |
| 228 | |
| 229 | inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data) |
| 230 | { |
| 231 | return cfpkt_add_body(pkt, &data, 1); |
| 232 | } |
| 233 | EXPORT_SYMBOL(cfpkt_addbdy); |
| 234 | |
| 235 | int cfpkt_add_head(struct cfpkt *pkt, const void *data2, u16 len) |
| 236 | { |
| 237 | struct sk_buff *skb = pkt_to_skb(pkt); |
| 238 | struct sk_buff *lastskb; |
| 239 | u8 *to; |
| 240 | const u8 *data = data2; |
Sjur Braendeland | 638e628 | 2010-05-21 02:16:09 +0000 | [diff] [blame] | 241 | int ret; |
Sjur Braendeland | 15c9ac0 | 2010-03-30 13:56:24 +0000 | [diff] [blame] | 242 | if (unlikely(is_erronous(pkt))) |
| 243 | return -EPROTO; |
| 244 | if (unlikely(skb_headroom(skb) < len)) { |
| 245 | PKT_ERROR(pkt, "cfpkt_add_head: no headroom\n"); |
| 246 | return -EPROTO; |
| 247 | } |
| 248 | |
| 249 | /* Make sure data is writable */ |
Sjur Braendeland | 638e628 | 2010-05-21 02:16:09 +0000 | [diff] [blame] | 250 | ret = skb_cow_data(skb, 0, &lastskb); |
| 251 | if (unlikely(ret < 0)) { |
Sjur Braendeland | 15c9ac0 | 2010-03-30 13:56:24 +0000 | [diff] [blame] | 252 | PKT_ERROR(pkt, "cfpkt_add_head: cow failed\n"); |
Sjur Braendeland | 638e628 | 2010-05-21 02:16:09 +0000 | [diff] [blame] | 253 | return ret; |
Sjur Braendeland | 15c9ac0 | 2010-03-30 13:56:24 +0000 | [diff] [blame] | 254 | } |
| 255 | |
| 256 | to = skb_push(skb, len); |
| 257 | memcpy(to, data, len); |
| 258 | return 0; |
| 259 | } |
| 260 | EXPORT_SYMBOL(cfpkt_add_head); |
| 261 | |
| 262 | inline int cfpkt_add_trail(struct cfpkt *pkt, const void *data, u16 len) |
| 263 | { |
| 264 | return cfpkt_add_body(pkt, data, len); |
| 265 | } |
| 266 | EXPORT_SYMBOL(cfpkt_add_trail); |
| 267 | |
| 268 | inline u16 cfpkt_getlen(struct cfpkt *pkt) |
| 269 | { |
| 270 | struct sk_buff *skb = pkt_to_skb(pkt); |
| 271 | return skb->len; |
| 272 | } |
| 273 | EXPORT_SYMBOL(cfpkt_getlen); |
| 274 | |
| 275 | inline u16 cfpkt_iterate(struct cfpkt *pkt, |
| 276 | u16 (*iter_func)(u16, void *, u16), |
| 277 | u16 data) |
| 278 | { |
| 279 | /* |
| 280 | * Don't care about the performance hit of linearizing, |
| 281 | * Checksum should not be used on high-speed interfaces anyway. |
| 282 | */ |
| 283 | if (unlikely(is_erronous(pkt))) |
| 284 | return -EPROTO; |
| 285 | if (unlikely(skb_linearize(&pkt->skb) != 0)) { |
| 286 | PKT_ERROR(pkt, "cfpkt_iterate: linearize failed\n"); |
| 287 | return -EPROTO; |
| 288 | } |
| 289 | return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt)); |
| 290 | } |
| 291 | EXPORT_SYMBOL(cfpkt_iterate); |
| 292 | |
| 293 | int cfpkt_setlen(struct cfpkt *pkt, u16 len) |
| 294 | { |
| 295 | struct sk_buff *skb = pkt_to_skb(pkt); |
| 296 | |
| 297 | |
| 298 | if (unlikely(is_erronous(pkt))) |
| 299 | return -EPROTO; |
| 300 | |
| 301 | if (likely(len <= skb->len)) { |
| 302 | if (unlikely(skb->data_len)) |
| 303 | ___pskb_trim(skb, len); |
| 304 | else |
| 305 | skb_trim(skb, len); |
| 306 | |
| 307 | return cfpkt_getlen(pkt); |
| 308 | } |
| 309 | |
| 310 | /* Need to expand SKB */ |
| 311 | if (unlikely(!cfpkt_pad_trail(pkt, len - skb->len))) |
| 312 | PKT_ERROR(pkt, "cfpkt_setlen: skb_pad_trail failed\n"); |
| 313 | |
| 314 | return cfpkt_getlen(pkt); |
| 315 | } |
| 316 | EXPORT_SYMBOL(cfpkt_setlen); |
| 317 | |
| 318 | struct cfpkt *cfpkt_create_uplink(const unsigned char *data, unsigned int len) |
| 319 | { |
| 320 | struct cfpkt *pkt = cfpkt_create_pfx(len + PKT_POSTFIX, PKT_PREFIX); |
Sjur Braendeland | 638e628 | 2010-05-21 02:16:09 +0000 | [diff] [blame] | 321 | if (!pkt) |
| 322 | return NULL; |
Sjur Braendeland | 15c9ac0 | 2010-03-30 13:56:24 +0000 | [diff] [blame] | 323 | if (unlikely(data != NULL)) |
| 324 | cfpkt_add_body(pkt, data, len); |
| 325 | return pkt; |
| 326 | } |
| 327 | EXPORT_SYMBOL(cfpkt_create_uplink); |
| 328 | |
| 329 | struct cfpkt *cfpkt_append(struct cfpkt *dstpkt, |
| 330 | struct cfpkt *addpkt, |
| 331 | u16 expectlen) |
| 332 | { |
| 333 | struct sk_buff *dst = pkt_to_skb(dstpkt); |
| 334 | struct sk_buff *add = pkt_to_skb(addpkt); |
| 335 | u16 addlen = skb_headlen(add); |
| 336 | u16 neededtailspace; |
| 337 | struct sk_buff *tmp; |
| 338 | u16 dstlen; |
| 339 | u16 createlen; |
| 340 | if (unlikely(is_erronous(dstpkt) || is_erronous(addpkt))) { |
Sjur Braendeland | 15c9ac0 | 2010-03-30 13:56:24 +0000 | [diff] [blame] | 341 | return dstpkt; |
| 342 | } |
| 343 | if (expectlen > addlen) |
| 344 | neededtailspace = expectlen; |
| 345 | else |
| 346 | neededtailspace = addlen; |
| 347 | |
| 348 | if (dst->tail + neededtailspace > dst->end) { |
| 349 | /* Create a dumplicate of 'dst' with more tail space */ |
Sjur Braendeland | 638e628 | 2010-05-21 02:16:09 +0000 | [diff] [blame] | 350 | struct cfpkt *tmppkt; |
Sjur Braendeland | 15c9ac0 | 2010-03-30 13:56:24 +0000 | [diff] [blame] | 351 | dstlen = skb_headlen(dst); |
| 352 | createlen = dstlen + neededtailspace; |
Sjur Braendeland | 638e628 | 2010-05-21 02:16:09 +0000 | [diff] [blame] | 353 | tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX); |
| 354 | if (tmppkt == NULL) |
Sjur Braendeland | 15c9ac0 | 2010-03-30 13:56:24 +0000 | [diff] [blame] | 355 | return NULL; |
Sjur Braendeland | 638e628 | 2010-05-21 02:16:09 +0000 | [diff] [blame] | 356 | tmp = pkt_to_skb(tmppkt); |
Sjur Braendeland | 15c9ac0 | 2010-03-30 13:56:24 +0000 | [diff] [blame] | 357 | skb_set_tail_pointer(tmp, dstlen); |
| 358 | tmp->len = dstlen; |
| 359 | memcpy(tmp->data, dst->data, dstlen); |
| 360 | cfpkt_destroy(dstpkt); |
| 361 | dst = tmp; |
| 362 | } |
| 363 | memcpy(skb_tail_pointer(dst), add->data, skb_headlen(add)); |
| 364 | cfpkt_destroy(addpkt); |
| 365 | dst->tail += addlen; |
| 366 | dst->len += addlen; |
| 367 | return skb_to_pkt(dst); |
| 368 | } |
| 369 | EXPORT_SYMBOL(cfpkt_append); |
| 370 | |
| 371 | struct cfpkt *cfpkt_split(struct cfpkt *pkt, u16 pos) |
| 372 | { |
| 373 | struct sk_buff *skb2; |
| 374 | struct sk_buff *skb = pkt_to_skb(pkt); |
Sjur Braendeland | 638e628 | 2010-05-21 02:16:09 +0000 | [diff] [blame] | 375 | struct cfpkt *tmppkt; |
Sjur Braendeland | 15c9ac0 | 2010-03-30 13:56:24 +0000 | [diff] [blame] | 376 | u8 *split = skb->data + pos; |
| 377 | u16 len2nd = skb_tail_pointer(skb) - split; |
| 378 | |
| 379 | if (unlikely(is_erronous(pkt))) |
| 380 | return NULL; |
| 381 | |
| 382 | if (skb->data + pos > skb_tail_pointer(skb)) { |
| 383 | PKT_ERROR(pkt, |
| 384 | "cfpkt_split: trying to split beyond end of packet"); |
| 385 | return NULL; |
| 386 | } |
| 387 | |
| 388 | /* Create a new packet for the second part of the data */ |
Sjur Braendeland | 638e628 | 2010-05-21 02:16:09 +0000 | [diff] [blame] | 389 | tmppkt = cfpkt_create_pfx(len2nd + PKT_PREFIX + PKT_POSTFIX, |
| 390 | PKT_PREFIX); |
| 391 | if (tmppkt == NULL) |
| 392 | return NULL; |
| 393 | skb2 = pkt_to_skb(tmppkt); |
| 394 | |
Sjur Braendeland | 15c9ac0 | 2010-03-30 13:56:24 +0000 | [diff] [blame] | 395 | |
| 396 | if (skb2 == NULL) |
| 397 | return NULL; |
| 398 | |
| 399 | /* Reduce the length of the original packet */ |
| 400 | skb_set_tail_pointer(skb, pos); |
| 401 | skb->len = pos; |
| 402 | |
| 403 | memcpy(skb2->data, split, len2nd); |
| 404 | skb2->tail += len2nd; |
| 405 | skb2->len += len2nd; |
| 406 | return skb_to_pkt(skb2); |
| 407 | } |
| 408 | EXPORT_SYMBOL(cfpkt_split); |
| 409 | |
| 410 | char *cfpkt_log_pkt(struct cfpkt *pkt, char *buf, int buflen) |
| 411 | { |
| 412 | struct sk_buff *skb = pkt_to_skb(pkt); |
| 413 | char *p = buf; |
| 414 | int i; |
| 415 | |
| 416 | /* |
| 417 | * Sanity check buffer length, it needs to be at least as large as |
| 418 | * the header info: ~=50+ bytes |
| 419 | */ |
| 420 | if (buflen < 50) |
| 421 | return NULL; |
| 422 | |
| 423 | snprintf(buf, buflen, "%s: pkt:%p len:%ld(%ld+%ld) {%ld,%ld} data: [", |
| 424 | is_erronous(pkt) ? "ERRONOUS-SKB" : |
| 425 | (skb->data_len != 0 ? "COMPLEX-SKB" : "SKB"), |
| 426 | skb, |
| 427 | (long) skb->len, |
| 428 | (long) (skb_tail_pointer(skb) - skb->data), |
| 429 | (long) skb->data_len, |
| 430 | (long) (skb->data - skb->head), |
| 431 | (long) (skb_tail_pointer(skb) - skb->head)); |
| 432 | p = buf + strlen(buf); |
| 433 | |
| 434 | for (i = 0; i < skb_tail_pointer(skb) - skb->data && i < 300; i++) { |
| 435 | if (p > buf + buflen - 10) { |
| 436 | sprintf(p, "..."); |
| 437 | p = buf + strlen(buf); |
| 438 | break; |
| 439 | } |
| 440 | sprintf(p, "%02x,", skb->data[i]); |
| 441 | p = buf + strlen(buf); |
| 442 | } |
| 443 | sprintf(p, "]\n"); |
| 444 | return buf; |
| 445 | } |
| 446 | EXPORT_SYMBOL(cfpkt_log_pkt); |
| 447 | |
| 448 | int cfpkt_raw_append(struct cfpkt *pkt, void **buf, unsigned int buflen) |
| 449 | { |
| 450 | struct sk_buff *skb = pkt_to_skb(pkt); |
| 451 | struct sk_buff *lastskb; |
| 452 | |
| 453 | caif_assert(buf != NULL); |
| 454 | if (unlikely(is_erronous(pkt))) |
| 455 | return -EPROTO; |
| 456 | /* Make sure SKB is writable */ |
| 457 | if (unlikely(skb_cow_data(skb, 0, &lastskb) < 0)) { |
| 458 | PKT_ERROR(pkt, "cfpkt_raw_append: skb_cow_data failed\n"); |
| 459 | return -EPROTO; |
| 460 | } |
| 461 | |
| 462 | if (unlikely(skb_linearize(skb) != 0)) { |
| 463 | PKT_ERROR(pkt, "cfpkt_raw_append: linearize failed\n"); |
| 464 | return -EPROTO; |
| 465 | } |
| 466 | |
| 467 | if (unlikely(skb_tailroom(skb) < buflen)) { |
| 468 | PKT_ERROR(pkt, "cfpkt_raw_append: buffer too short - failed\n"); |
| 469 | return -EPROTO; |
| 470 | } |
| 471 | |
| 472 | *buf = skb_put(skb, buflen); |
| 473 | return 1; |
| 474 | } |
| 475 | EXPORT_SYMBOL(cfpkt_raw_append); |
| 476 | |
| 477 | int cfpkt_raw_extract(struct cfpkt *pkt, void **buf, unsigned int buflen) |
| 478 | { |
| 479 | struct sk_buff *skb = pkt_to_skb(pkt); |
| 480 | |
| 481 | caif_assert(buf != NULL); |
| 482 | if (unlikely(is_erronous(pkt))) |
| 483 | return -EPROTO; |
| 484 | |
| 485 | if (unlikely(buflen > skb->len)) { |
| 486 | PKT_ERROR(pkt, "cfpkt_raw_extract: buflen too large " |
| 487 | "- failed\n"); |
| 488 | return -EPROTO; |
| 489 | } |
| 490 | |
| 491 | if (unlikely(buflen > skb_headlen(skb))) { |
| 492 | if (unlikely(skb_linearize(skb) != 0)) { |
| 493 | PKT_ERROR(pkt, "cfpkt_raw_extract: linearize failed\n"); |
| 494 | return -EPROTO; |
| 495 | } |
| 496 | } |
| 497 | |
| 498 | *buf = skb->data; |
| 499 | skb_pull(skb, buflen); |
| 500 | |
| 501 | return 1; |
| 502 | } |
| 503 | EXPORT_SYMBOL(cfpkt_raw_extract); |
| 504 | |
| 505 | inline bool cfpkt_erroneous(struct cfpkt *pkt) |
| 506 | { |
| 507 | return cfpkt_priv(pkt)->erronous; |
| 508 | } |
| 509 | EXPORT_SYMBOL(cfpkt_erroneous); |
| 510 | |
| 511 | struct cfpktq *cfpktq_create(void) |
| 512 | { |
| 513 | struct cfpktq *q = kmalloc(sizeof(struct cfpktq), GFP_ATOMIC); |
| 514 | if (!q) |
| 515 | return NULL; |
| 516 | skb_queue_head_init(&q->head); |
| 517 | atomic_set(&q->count, 0); |
| 518 | spin_lock_init(&q->lock); |
| 519 | return q; |
| 520 | } |
| 521 | EXPORT_SYMBOL(cfpktq_create); |
| 522 | |
| 523 | void cfpkt_queue(struct cfpktq *pktq, struct cfpkt *pkt, unsigned short prio) |
| 524 | { |
| 525 | atomic_inc(&pktq->count); |
| 526 | spin_lock(&pktq->lock); |
| 527 | skb_queue_tail(&pktq->head, pkt_to_skb(pkt)); |
| 528 | spin_unlock(&pktq->lock); |
| 529 | |
| 530 | } |
| 531 | EXPORT_SYMBOL(cfpkt_queue); |
| 532 | |
| 533 | struct cfpkt *cfpkt_qpeek(struct cfpktq *pktq) |
| 534 | { |
| 535 | struct cfpkt *tmp; |
| 536 | spin_lock(&pktq->lock); |
| 537 | tmp = skb_to_pkt(skb_peek(&pktq->head)); |
| 538 | spin_unlock(&pktq->lock); |
| 539 | return tmp; |
| 540 | } |
| 541 | EXPORT_SYMBOL(cfpkt_qpeek); |
| 542 | |
| 543 | struct cfpkt *cfpkt_dequeue(struct cfpktq *pktq) |
| 544 | { |
| 545 | struct cfpkt *pkt; |
| 546 | spin_lock(&pktq->lock); |
| 547 | pkt = skb_to_pkt(skb_dequeue(&pktq->head)); |
| 548 | if (pkt) { |
| 549 | atomic_dec(&pktq->count); |
| 550 | caif_assert(atomic_read(&pktq->count) >= 0); |
| 551 | } |
| 552 | spin_unlock(&pktq->lock); |
| 553 | return pkt; |
| 554 | } |
| 555 | EXPORT_SYMBOL(cfpkt_dequeue); |
| 556 | |
| 557 | int cfpkt_qcount(struct cfpktq *pktq) |
| 558 | { |
| 559 | return atomic_read(&pktq->count); |
| 560 | } |
| 561 | EXPORT_SYMBOL(cfpkt_qcount); |
| 562 | |
| 563 | struct cfpkt *cfpkt_clone_release(struct cfpkt *pkt) |
| 564 | { |
| 565 | struct cfpkt *clone; |
| 566 | clone = skb_to_pkt(skb_clone(pkt_to_skb(pkt), GFP_ATOMIC)); |
| 567 | /* Free original packet. */ |
| 568 | cfpkt_destroy(pkt); |
| 569 | if (!clone) |
| 570 | return NULL; |
| 571 | return clone; |
| 572 | } |
| 573 | EXPORT_SYMBOL(cfpkt_clone_release); |
| 574 | |
| 575 | struct caif_payload_info *cfpkt_info(struct cfpkt *pkt) |
| 576 | { |
| 577 | return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb; |
| 578 | } |
| 579 | EXPORT_SYMBOL(cfpkt_info); |