net/caif/cfpkt_skbuff.c

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