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 <linux/export.h>
  13 #include <net/caif/cfpkt.h>
  14
  15 #define PKT_PREFIX  48
  16 #define PKT_POSTFIX 2
  17 #define PKT_LEN_WHEN_EXTENDING 128
  18 #define PKT_ERROR(pkt, errmsg)             \
  19 do {                                       \
  20         cfpkt_priv(pkt)->erronous = true;  \
  21         skb_reset_tail_pointer(&pkt->skb); \
  22         pr_warn(errmsg);                   \
  23 } while (0)
  24
  25 struct cfpktq {
  26         struct sk_buff_head head;
  27         atomic_t count;
  28         /* Lock protects count updates */
  29         spinlock_t lock;
  30 };
  31
  32 /*
  33  * net/caif/ is generic and does not
  34  * understand SKB, so we do this typecast
  35  */
  36 struct cfpkt {
  37         struct sk_buff skb;
  38 };
  39
  40 /* Private data inside SKB */
  41 struct cfpkt_priv_data {
  42         struct dev_info dev_info;
  43         bool erronous;
  44 };
  45
  46 static inline struct cfpkt_priv_data *cfpkt_priv(struct cfpkt *pkt)
  47 {
  48         return (struct cfpkt_priv_data *) pkt->skb.cb;
  49 }
  50
  51 static inline bool is_erronous(struct cfpkt *pkt)
  52 {
  53         return cfpkt_priv(pkt)->erronous;
  54 }
  55
  56 static inline struct sk_buff *pkt_to_skb(struct cfpkt *pkt)
  57 {
  58         return &pkt->skb;
  59 }
  60
  61 static inline struct cfpkt *skb_to_pkt(struct sk_buff *skb)
  62 {
  63         return (struct cfpkt *) skb;
  64 }
  65
  66
  67 struct cfpkt *cfpkt_fromnative(enum caif_direction dir, void *nativepkt)
  68 {
  69         struct cfpkt *pkt = skb_to_pkt(nativepkt);
  70         cfpkt_priv(pkt)->erronous = false;
  71         return pkt;
  72 }
  73 EXPORT_SYMBOL(cfpkt_fromnative);
  74
  75 void *cfpkt_tonative(struct cfpkt *pkt)
  76 {
  77         return (void *) pkt;
  78 }
  79 EXPORT_SYMBOL(cfpkt_tonative);
  80
  81 static struct cfpkt *cfpkt_create_pfx(u16 len, u16 pfx)
  82 {
  83         struct sk_buff *skb;
  84
  85         if (likely(in_interrupt()))
  86                 skb = alloc_skb(len + pfx, GFP_ATOMIC);
  87         else
  88                 skb = alloc_skb(len + pfx, GFP_KERNEL);
  89
  90         if (unlikely(skb == NULL))
  91                 return NULL;
  92
  93         skb_reserve(skb, pfx);
  94         return skb_to_pkt(skb);
  95 }
  96
  97 inline struct cfpkt *cfpkt_create(u16 len)
  98 {
  99         return cfpkt_create_pfx(len + PKT_POSTFIX, PKT_PREFIX);
 100 }
 101
 102 void cfpkt_destroy(struct cfpkt *pkt)
 103 {
 104         struct sk_buff *skb = pkt_to_skb(pkt);
 105         kfree_skb(skb);
 106 }
 107
 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
 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
 127 int cfpkt_extr_head(struct cfpkt *pkt, void *data, u16 len)
 128 {
 129         struct sk_buff *skb = pkt_to_skb(pkt);
 130         u8 *from;
 131         if (unlikely(is_erronous(pkt)))
 132                 return -EPROTO;
 133
 134         if (unlikely(len > skb->len)) {
 135                 PKT_ERROR(pkt, "read beyond end of packet\n");
 136                 return -EPROTO;
 137         }
 138
 139         if (unlikely(len > skb_headlen(skb))) {
 140                 if (unlikely(skb_linearize(skb) != 0)) {
 141                         PKT_ERROR(pkt, "linearize failed\n");
 142                         return -EPROTO;
 143                 }
 144         }
 145         from = skb_pull(skb, len);
 146         from -= len;
 147         if (data)
 148                 memcpy(data, from, len);
 149         return 0;
 150 }
 151
 152 int cfpkt_extr_trail(struct cfpkt *pkt, void *dta, u16 len)
 153 {
 154         struct sk_buff *skb = pkt_to_skb(pkt);
 155         u8 *data = dta;
 156         u8 *from;
 157         if (unlikely(is_erronous(pkt)))
 158                 return -EPROTO;
 159
 160         if (unlikely(skb_linearize(skb) != 0)) {
 161                 PKT_ERROR(pkt, "linearize failed\n");
 162                 return -EPROTO;
 163         }
 164         if (unlikely(skb->data + len > skb_tail_pointer(skb))) {
 165                 PKT_ERROR(pkt, "read beyond end of packet\n");
 166                 return -EPROTO;
 167         }
 168         from = skb_tail_pointer(skb) - len;
 169         skb_trim(skb, skb->len - len);
 170         memcpy(data, from, len);
 171         return 0;
 172 }
 173
 174
 175 int cfpkt_pad_trail(struct cfpkt *pkt, u16 len)
 176 {
 177         return cfpkt_add_body(pkt, NULL, len);
 178 }
 179
 180
 181 int cfpkt_add_body(struct cfpkt *pkt, const void *data, u16 len)
 182 {
 183         struct sk_buff *skb = pkt_to_skb(pkt);
 184         struct sk_buff *lastskb;
 185         u8 *to;
 186         u16 addlen = 0;
 187
 188
 189         if (unlikely(is_erronous(pkt)))
 190                 return -EPROTO;
 191
 192         lastskb = skb;
 193
 194         /* Check whether we need to add space at the tail */
 195         if (unlikely(skb_tailroom(skb) < len)) {
 196                 if (likely(len < PKT_LEN_WHEN_EXTENDING))
 197                         addlen = PKT_LEN_WHEN_EXTENDING;
 198                 else
 199                         addlen = len;
 200         }
 201
 202         /* Check whether we need to change the SKB before writing to the tail */
 203         if (unlikely((addlen > 0) || skb_cloned(skb) || skb_shared(skb))) {
 204
 205                 /* Make sure data is writable */
 206                 if (unlikely(skb_cow_data(skb, addlen, &lastskb) < 0)) {
 207                         PKT_ERROR(pkt, "cow failed\n");
 208                         return -EPROTO;
 209                 }
 210                 /*
 211                  * Is the SKB non-linear after skb_cow_data()? If so, we are
 212                  * going to add data to the last SKB, so we need to adjust
 213                  * lengths of the top SKB.
 214                  */
 215                 if (lastskb != skb) {
 216                         pr_warn("Packet is non-linear\n");
 217                         skb->len += len;
 218                         skb->data_len += len;
 219                 }
 220         }
 221
 222         /* All set to put the last SKB and optionally write data there. */
 223         to = skb_put(lastskb, len);
 224         if (likely(data))
 225                 memcpy(to, data, len);
 226         return 0;
 227 }
 228
 229 inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data)
 230 {
 231         return cfpkt_add_body(pkt, &data, 1);
 232 }
 233
 234 int cfpkt_add_head(struct cfpkt *pkt, const void *data2, u16 len)
 235 {
 236         struct sk_buff *skb = pkt_to_skb(pkt);
 237         struct sk_buff *lastskb;
 238         u8 *to;
 239         const u8 *data = data2;
 240         int ret;
 241         if (unlikely(is_erronous(pkt)))
 242                 return -EPROTO;
 243         if (unlikely(skb_headroom(skb) < len)) {
 244                 PKT_ERROR(pkt, "no headroom\n");
 245                 return -EPROTO;
 246         }
 247
 248         /* Make sure data is writable */
 249         ret = skb_cow_data(skb, 0, &lastskb);
 250         if (unlikely(ret < 0)) {
 251                 PKT_ERROR(pkt, "cow failed\n");
 252                 return ret;
 253         }
 254
 255         to = skb_push(skb, len);
 256         memcpy(to, data, len);
 257         return 0;
 258 }
 259
 260
 261 inline int cfpkt_add_trail(struct cfpkt *pkt, const void *data, u16 len)
 262 {
 263         return cfpkt_add_body(pkt, data, len);
 264 }
 265
 266
 267 inline u16 cfpkt_getlen(struct cfpkt *pkt)
 268 {
 269         struct sk_buff *skb = pkt_to_skb(pkt);
 270         return skb->len;
 271 }
 272
 273
 274 inline u16 cfpkt_iterate(struct cfpkt *pkt,
 275                             u16 (*iter_func)(u16, void *, u16),
 276                             u16 data)
 277 {
 278         /*
 279          * Don't care about the performance hit of linearizing,
 280          * Checksum should not be used on high-speed interfaces anyway.
 281          */
 282         if (unlikely(is_erronous(pkt)))
 283                 return -EPROTO;
 284         if (unlikely(skb_linearize(&pkt->skb) != 0)) {
 285                 PKT_ERROR(pkt, "linearize failed\n");
 286                 return -EPROTO;
 287         }
 288         return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt));
 289 }
 290
 291
 292 int cfpkt_setlen(struct cfpkt *pkt, u16 len)
 293 {
 294         struct sk_buff *skb = pkt_to_skb(pkt);
 295
 296
 297         if (unlikely(is_erronous(pkt)))
 298                 return -EPROTO;
 299
 300         if (likely(len <= skb->len)) {
 301                 if (unlikely(skb->data_len))
 302                         ___pskb_trim(skb, len);
 303                 else
 304                         skb_trim(skb, len);
 305
 306                         return cfpkt_getlen(pkt);
 307         }
 308
 309         /* Need to expand SKB */
 310         if (unlikely(!cfpkt_pad_trail(pkt, len - skb->len)))
 311                 PKT_ERROR(pkt, "skb_pad_trail failed\n");
 312
 313         return cfpkt_getlen(pkt);
 314 }
 315
 316 struct cfpkt *cfpkt_append(struct cfpkt *dstpkt,
 317                              struct cfpkt *addpkt,
 318                              u16 expectlen)
 319 {
 320         struct sk_buff *dst = pkt_to_skb(dstpkt);
 321         struct sk_buff *add = pkt_to_skb(addpkt);
 322         u16 addlen = skb_headlen(add);
 323         u16 neededtailspace;
 324         struct sk_buff *tmp;
 325         u16 dstlen;
 326         u16 createlen;
 327         if (unlikely(is_erronous(dstpkt) || is_erronous(addpkt))) {
 328                 return dstpkt;
 329         }
 330         if (expectlen > addlen)
 331                 neededtailspace = expectlen;
 332         else
 333                 neededtailspace = addlen;
 334
 335         if (dst->tail + neededtailspace > dst->end) {
 336                 /* Create a dumplicate of 'dst' with more tail space */
 337                 struct cfpkt *tmppkt;
 338                 dstlen = skb_headlen(dst);
 339                 createlen = dstlen + neededtailspace;
 340                 tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX);
 341                 if (tmppkt == NULL)
 342                         return NULL;
 343                 tmp = pkt_to_skb(tmppkt);
 344                 skb_set_tail_pointer(tmp, dstlen);
 345                 tmp->len = dstlen;
 346                 memcpy(tmp->data, dst->data, dstlen);
 347                 cfpkt_destroy(dstpkt);
 348                 dst = tmp;
 349         }
 350         memcpy(skb_tail_pointer(dst), add->data, skb_headlen(add));
 351         cfpkt_destroy(addpkt);
 352         dst->tail += addlen;
 353         dst->len += addlen;
 354         return skb_to_pkt(dst);
 355 }
 356
 357 struct cfpkt *cfpkt_split(struct cfpkt *pkt, u16 pos)
 358 {
 359         struct sk_buff *skb2;
 360         struct sk_buff *skb = pkt_to_skb(pkt);
 361         struct cfpkt *tmppkt;
 362         u8 *split = skb->data + pos;
 363         u16 len2nd = skb_tail_pointer(skb) - split;
 364
 365         if (unlikely(is_erronous(pkt)))
 366                 return NULL;
 367
 368         if (skb->data + pos > skb_tail_pointer(skb)) {
 369                 PKT_ERROR(pkt, "trying to split beyond end of packet\n");
 370                 return NULL;
 371         }
 372
 373         /* Create a new packet for the second part of the data */
 374         tmppkt = cfpkt_create_pfx(len2nd + PKT_PREFIX + PKT_POSTFIX,
 375                                   PKT_PREFIX);
 376         if (tmppkt == NULL)
 377                 return NULL;
 378         skb2 = pkt_to_skb(tmppkt);
 379
 380
 381         if (skb2 == NULL)
 382                 return NULL;
 383
 384         /* Reduce the length of the original packet */
 385         skb_set_tail_pointer(skb, pos);
 386         skb->len = pos;
 387
 388         memcpy(skb2->data, split, len2nd);
 389         skb2->tail += len2nd;
 390         skb2->len += len2nd;
 391         return skb_to_pkt(skb2);
 392 }
 393
 394 bool cfpkt_erroneous(struct cfpkt *pkt)
 395 {
 396         return cfpkt_priv(pkt)->erronous;
 397 }
 398
 399 struct caif_payload_info *cfpkt_info(struct cfpkt *pkt)
 400 {
 401         return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb;
 402 }