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 #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 CAIF_NEEDED_HEADROOM
  13 #define PKT_POSTFIX CAIF_NEEDED_TAILROOM
  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                 cfpkt_destroy(addpkt);
 342                 return dstpkt;
 343         }
 344         if (expectlen > addlen)
 345                 neededtailspace = expectlen;
 346         else
 347                 neededtailspace = addlen;
 348
 349         if (dst->tail + neededtailspace > dst->end) {
 350                 /* Create a dumplicate of 'dst' with more tail space */
 351                 struct cfpkt *tmppkt;
 352                 dstlen = skb_headlen(dst);
 353                 createlen = dstlen + neededtailspace;
 354                 tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX);
 355                 if (tmppkt == NULL)
 356                         return NULL;
 357                 tmp = pkt_to_skb(tmppkt);
 358                 skb_set_tail_pointer(tmp, dstlen);
 359                 tmp->len = dstlen;
 360                 memcpy(tmp->data, dst->data, dstlen);
 361                 cfpkt_destroy(dstpkt);
 362                 dst = tmp;
 363         }
 364         memcpy(skb_tail_pointer(dst), add->data, skb_headlen(add));
 365         cfpkt_destroy(addpkt);
 366         dst->tail += addlen;
 367         dst->len += addlen;
 368         return skb_to_pkt(dst);
 369 }
 370 EXPORT_SYMBOL(cfpkt_append);
 371
 372 struct cfpkt *cfpkt_split(struct cfpkt *pkt, u16 pos)
 373 {
 374         struct sk_buff *skb2;
 375         struct sk_buff *skb = pkt_to_skb(pkt);
 376         struct cfpkt *tmppkt;
 377         u8 *split = skb->data + pos;
 378         u16 len2nd = skb_tail_pointer(skb) - split;
 379
 380         if (unlikely(is_erronous(pkt)))
 381                 return NULL;
 382
 383         if (skb->data + pos > skb_tail_pointer(skb)) {
 384                 PKT_ERROR(pkt,
 385                           "cfpkt_split: trying to split beyond end of packet");
 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, "cfpkt_raw_append: skb_cow_data failed\n");
 460                 return -EPROTO;
 461         }
 462
 463         if (unlikely(skb_linearize(skb) != 0)) {
 464                 PKT_ERROR(pkt, "cfpkt_raw_append: linearize failed\n");
 465                 return -EPROTO;
 466         }
 467
 468         if (unlikely(skb_tailroom(skb) < buflen)) {
 469                 PKT_ERROR(pkt, "cfpkt_raw_append: 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, "cfpkt_raw_extract: buflen too large "
 488                                 "- failed\n");
 489                 return -EPROTO;
 490         }
 491
 492         if (unlikely(buflen > skb_headlen(skb))) {
 493                 if (unlikely(skb_linearize(skb) != 0)) {
 494                         PKT_ERROR(pkt, "cfpkt_raw_extract: linearize failed\n");
 495                         return -EPROTO;
 496                 }
 497         }
 498
 499         *buf = skb->data;
 500         skb_pull(skb, buflen);
 501
 502         return 1;
 503 }
 504 EXPORT_SYMBOL(cfpkt_raw_extract);
 505
 506 inline bool cfpkt_erroneous(struct cfpkt *pkt)
 507 {
 508         return cfpkt_priv(pkt)->erronous;
 509 }
 510 EXPORT_SYMBOL(cfpkt_erroneous);
 511
 512 struct cfpktq *cfpktq_create(void)
 513 {
 514         struct cfpktq *q = kmalloc(sizeof(struct cfpktq), GFP_ATOMIC);
 515         if (!q)
 516                 return NULL;
 517         skb_queue_head_init(&q->head);
 518         atomic_set(&q->count, 0);
 519         spin_lock_init(&q->lock);
 520         return q;
 521 }
 522 EXPORT_SYMBOL(cfpktq_create);
 523
 524 void cfpkt_queue(struct cfpktq *pktq, struct cfpkt *pkt, unsigned short prio)
 525 {
 526         atomic_inc(&pktq->count);
 527         spin_lock(&pktq->lock);
 528         skb_queue_tail(&pktq->head, pkt_to_skb(pkt));
 529         spin_unlock(&pktq->lock);
 530
 531 }
 532 EXPORT_SYMBOL(cfpkt_queue);
 533
 534 struct cfpkt *cfpkt_qpeek(struct cfpktq *pktq)
 535 {
 536         struct cfpkt *tmp;
 537         spin_lock(&pktq->lock);
 538         tmp = skb_to_pkt(skb_peek(&pktq->head));
 539         spin_unlock(&pktq->lock);
 540         return tmp;
 541 }
 542 EXPORT_SYMBOL(cfpkt_qpeek);
 543
 544 struct cfpkt *cfpkt_dequeue(struct cfpktq *pktq)
 545 {
 546         struct cfpkt *pkt;
 547         spin_lock(&pktq->lock);
 548         pkt = skb_to_pkt(skb_dequeue(&pktq->head));
 549         if (pkt) {
 550                 atomic_dec(&pktq->count);
 551                 caif_assert(atomic_read(&pktq->count) >= 0);
 552         }
 553         spin_unlock(&pktq->lock);
 554         return pkt;
 555 }
 556 EXPORT_SYMBOL(cfpkt_dequeue);
 557
 558 int cfpkt_qcount(struct cfpktq *pktq)
 559 {
 560         return atomic_read(&pktq->count);
 561 }
 562 EXPORT_SYMBOL(cfpkt_qcount);
 563
 564 struct cfpkt *cfpkt_clone_release(struct cfpkt *pkt)
 565 {
 566         struct cfpkt *clone;
 567         clone  = skb_to_pkt(skb_clone(pkt_to_skb(pkt), GFP_ATOMIC));
 568         /* Free original packet. */
 569         cfpkt_destroy(pkt);
 570         if (!clone)
 571                 return NULL;
 572         return clone;
 573 }
 574 EXPORT_SYMBOL(cfpkt_clone_release);
 575
 576 struct caif_payload_info *cfpkt_info(struct cfpkt *pkt)
 577 {
 578         return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb;
 579 }
 580 EXPORT_SYMBOL(cfpkt_info);