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 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
 101 void cfpkt_destroy(struct cfpkt *pkt)
 102 {
 103         struct sk_buff *skb = pkt_to_skb(pkt);
 104         kfree_skb(skb);
 105 }
 106
 107 inline bool cfpkt_more(struct cfpkt *pkt)
 108 {
 109         struct sk_buff *skb = pkt_to_skb(pkt);
 110         return skb->len > 0;
 111 }
 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
 124 int cfpkt_extr_head(struct cfpkt *pkt, void *data, u16 len)
 125 {
 126         struct sk_buff *skb = pkt_to_skb(pkt);
 127         u8 *from;
 128         if (unlikely(is_erronous(pkt)))
 129                 return -EPROTO;
 130
 131         if (unlikely(len > skb->len)) {
 132                 PKT_ERROR(pkt, "read beyond end of packet\n");
 133                 return -EPROTO;
 134         }
 135
 136         if (unlikely(len > skb_headlen(skb))) {
 137                 if (unlikely(skb_linearize(skb) != 0)) {
 138                         PKT_ERROR(pkt, "linearize failed\n");
 139                         return -EPROTO;
 140                 }
 141         }
 142         from = skb_pull(skb, len);
 143         from -= len;
 144         if (data)
 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, "linearize failed\n");
 160                 return -EPROTO;
 161         }
 162         if (unlikely(skb->data + len > skb_tail_pointer(skb))) {
 163                 PKT_ERROR(pkt, "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
 172 int cfpkt_pad_trail(struct cfpkt *pkt, u16 len)
 173 {
 174         return cfpkt_add_body(pkt, NULL, len);
 175 }
 176
 177 int cfpkt_add_body(struct cfpkt *pkt, const void *data, u16 len)
 178 {
 179         struct sk_buff *skb = pkt_to_skb(pkt);
 180         struct sk_buff *lastskb;
 181         u8 *to;
 182         u16 addlen = 0;
 183
 184
 185         if (unlikely(is_erronous(pkt)))
 186                 return -EPROTO;
 187
 188         lastskb = skb;
 189
 190         /* Check whether we need to add space at the tail */
 191         if (unlikely(skb_tailroom(skb) < len)) {
 192                 if (likely(len < PKT_LEN_WHEN_EXTENDING))
 193                         addlen = PKT_LEN_WHEN_EXTENDING;
 194                 else
 195                         addlen = len;
 196         }
 197
 198         /* Check whether we need to change the SKB before writing to the tail */
 199         if (unlikely((addlen > 0) || skb_cloned(skb) || skb_shared(skb))) {
 200
 201                 /* Make sure data is writable */
 202                 if (unlikely(skb_cow_data(skb, addlen, &lastskb) < 0)) {
 203                         PKT_ERROR(pkt, "cow failed\n");
 204                         return -EPROTO;
 205                 }
 206                 /*
 207                  * Is the SKB non-linear after skb_cow_data()? If so, we are
 208                  * going to add data to the last SKB, so we need to adjust
 209                  * lengths of the top SKB.
 210                  */
 211                 if (lastskb != skb) {
 212                         pr_warn("Packet is non-linear\n");
 213                         skb->len += len;
 214                         skb->data_len += len;
 215                 }
 216         }
 217
 218         /* All set to put the last SKB and optionally write data there. */
 219         to = skb_put(lastskb, len);
 220         if (likely(data))
 221                 memcpy(to, data, len);
 222         return 0;
 223 }
 224
 225 inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data)
 226 {
 227         return cfpkt_add_body(pkt, &data, 1);
 228 }
 229
 230 int cfpkt_add_head(struct cfpkt *pkt, const void *data2, u16 len)
 231 {
 232         struct sk_buff *skb = pkt_to_skb(pkt);
 233         struct sk_buff *lastskb;
 234         u8 *to;
 235         const u8 *data = data2;
 236         int ret;
 237         if (unlikely(is_erronous(pkt)))
 238                 return -EPROTO;
 239         if (unlikely(skb_headroom(skb) < len)) {
 240                 PKT_ERROR(pkt, "no headroom\n");
 241                 return -EPROTO;
 242         }
 243
 244         /* Make sure data is writable */
 245         ret = skb_cow_data(skb, 0, &lastskb);
 246         if (unlikely(ret < 0)) {
 247                 PKT_ERROR(pkt, "cow failed\n");
 248                 return ret;
 249         }
 250
 251         to = skb_push(skb, len);
 252         memcpy(to, data, len);
 253         return 0;
 254 }
 255 EXPORT_SYMBOL(cfpkt_add_head);
 256
 257 inline int cfpkt_add_trail(struct cfpkt *pkt, const void *data, u16 len)
 258 {
 259         return cfpkt_add_body(pkt, data, len);
 260 }
 261
 262 inline u16 cfpkt_getlen(struct cfpkt *pkt)
 263 {
 264         struct sk_buff *skb = pkt_to_skb(pkt);
 265         return skb->len;
 266 }
 267
 268 inline u16 cfpkt_iterate(struct cfpkt *pkt,
 269                             u16 (*iter_func)(u16, void *, u16),
 270                             u16 data)
 271 {
 272         /*
 273          * Don't care about the performance hit of linearizing,
 274          * Checksum should not be used on high-speed interfaces anyway.
 275          */
 276         if (unlikely(is_erronous(pkt)))
 277                 return -EPROTO;
 278         if (unlikely(skb_linearize(&pkt->skb) != 0)) {
 279                 PKT_ERROR(pkt, "linearize failed\n");
 280                 return -EPROTO;
 281         }
 282         return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt));
 283 }
 284
 285 int cfpkt_setlen(struct cfpkt *pkt, u16 len)
 286 {
 287         struct sk_buff *skb = pkt_to_skb(pkt);
 288
 289
 290         if (unlikely(is_erronous(pkt)))
 291                 return -EPROTO;
 292
 293         if (likely(len <= skb->len)) {
 294                 if (unlikely(skb->data_len))
 295                         ___pskb_trim(skb, len);
 296                 else
 297                         skb_trim(skb, len);
 298
 299                         return cfpkt_getlen(pkt);
 300         }
 301
 302         /* Need to expand SKB */
 303         if (unlikely(!cfpkt_pad_trail(pkt, len - skb->len)))
 304                 PKT_ERROR(pkt, "skb_pad_trail failed\n");
 305
 306         return cfpkt_getlen(pkt);
 307 }
 308
 309 struct cfpkt *cfpkt_append(struct cfpkt *dstpkt,
 310                              struct cfpkt *addpkt,
 311                              u16 expectlen)
 312 {
 313         struct sk_buff *dst = pkt_to_skb(dstpkt);
 314         struct sk_buff *add = pkt_to_skb(addpkt);
 315         u16 addlen = skb_headlen(add);
 316         u16 neededtailspace;
 317         struct sk_buff *tmp;
 318         u16 dstlen;
 319         u16 createlen;
 320         if (unlikely(is_erronous(dstpkt) || is_erronous(addpkt))) {
 321                 return dstpkt;
 322         }
 323         if (expectlen > addlen)
 324                 neededtailspace = expectlen;
 325         else
 326                 neededtailspace = addlen;
 327
 328         if (dst->tail + neededtailspace > dst->end) {
 329                 /* Create a dumplicate of 'dst' with more tail space */
 330                 struct cfpkt *tmppkt;
 331                 dstlen = skb_headlen(dst);
 332                 createlen = dstlen + neededtailspace;
 333                 tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX);
 334                 if (tmppkt == NULL)
 335                         return NULL;
 336                 tmp = pkt_to_skb(tmppkt);
 337                 skb_set_tail_pointer(tmp, dstlen);
 338                 tmp->len = dstlen;
 339                 memcpy(tmp->data, dst->data, dstlen);
 340                 cfpkt_destroy(dstpkt);
 341                 dst = tmp;
 342         }
 343         memcpy(skb_tail_pointer(dst), add->data, skb_headlen(add));
 344         cfpkt_destroy(addpkt);
 345         dst->tail += addlen;
 346         dst->len += addlen;
 347         return skb_to_pkt(dst);
 348 }
 349
 350 struct cfpkt *cfpkt_split(struct cfpkt *pkt, u16 pos)
 351 {
 352         struct sk_buff *skb2;
 353         struct sk_buff *skb = pkt_to_skb(pkt);
 354         struct cfpkt *tmppkt;
 355         u8 *split = skb->data + pos;
 356         u16 len2nd = skb_tail_pointer(skb) - split;
 357
 358         if (unlikely(is_erronous(pkt)))
 359                 return NULL;
 360
 361         if (skb->data + pos > skb_tail_pointer(skb)) {
 362                 PKT_ERROR(pkt, "trying to split beyond end of packet\n");
 363                 return NULL;
 364         }
 365
 366         /* Create a new packet for the second part of the data */
 367         tmppkt = cfpkt_create_pfx(len2nd + PKT_PREFIX + PKT_POSTFIX,
 368                                   PKT_PREFIX);
 369         if (tmppkt == NULL)
 370                 return NULL;
 371         skb2 = pkt_to_skb(tmppkt);
 372
 373
 374         if (skb2 == NULL)
 375                 return NULL;
 376
 377         /* Reduce the length of the original packet */
 378         skb_set_tail_pointer(skb, pos);
 379         skb->len = pos;
 380
 381         memcpy(skb2->data, split, len2nd);
 382         skb2->tail += len2nd;
 383         skb2->len += len2nd;
 384         skb2->priority = skb->priority;
 385         return skb_to_pkt(skb2);
 386 }
 387
 388 bool cfpkt_erroneous(struct cfpkt *pkt)
 389 {
 390         return cfpkt_priv(pkt)->erronous;
 391 }
 392
 393 struct caif_payload_info *cfpkt_info(struct cfpkt *pkt)
 394 {
 395         return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb;
 396 }
 397 EXPORT_SYMBOL(cfpkt_info);
 398
 399 void cfpkt_set_prio(struct cfpkt *pkt, int prio)
 400 {
 401         pkt_to_skb(pkt)->priority = prio;
 402 }
 403 EXPORT_SYMBOL(cfpkt_set_prio);