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 12 #define PKT_PREFIX 48 13 #define PKT_POSTFIX 2 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; 241 int ret; 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 */ 250 ret = skb_cow_data(skb, 0, &lastskb); 251 if (unlikely(ret < 0)) { 252 PKT_ERROR(pkt, "cfpkt_add_head: cow failed\n"); 253 return ret; 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); 321 if (!pkt) 322 return NULL; 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))) { 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 */ 350 struct cfpkt *tmppkt; 351 dstlen = skb_headlen(dst); 352 createlen = dstlen + neededtailspace; 353 tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX); 354 if (tmppkt == NULL) 355 return NULL; 356 tmp = pkt_to_skb(tmppkt); 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); 375 struct cfpkt *tmppkt; 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 */ 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 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); 580