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