net/caif/cfpkt_skbuff.c

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