linux/net/caif/cfpkt_skbuff.c
<<
>>
Prefs
   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)             \
  19do {                                       \
  20        cfpkt_priv(pkt)->erronous = true;  \
  21        skb_reset_tail_pointer(&pkt->skb); \
  22        pr_warn(errmsg);                   \
  23} while (0)
  24
  25struct 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 */
  36struct cfpkt {
  37        struct sk_buff skb;
  38};
  39
  40/* Private data inside SKB */
  41struct cfpkt_priv_data {
  42        struct dev_info dev_info;
  43        bool erronous;
  44};
  45
  46static inline struct cfpkt_priv_data *cfpkt_priv(struct cfpkt *pkt)
  47{
  48        return (struct cfpkt_priv_data *) pkt->skb.cb;
  49}
  50
  51static inline bool is_erronous(struct cfpkt *pkt)
  52{
  53        return cfpkt_priv(pkt)->erronous;
  54}
  55
  56static inline struct sk_buff *pkt_to_skb(struct cfpkt *pkt)
  57{
  58        return &pkt->skb;
  59}
  60
  61static inline struct cfpkt *skb_to_pkt(struct sk_buff *skb)
  62{
  63        return (struct cfpkt *) skb;
  64}
  65
  66struct 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}
  72EXPORT_SYMBOL(cfpkt_fromnative);
  73
  74void *cfpkt_tonative(struct cfpkt *pkt)
  75{
  76        return (void *) pkt;
  77}
  78EXPORT_SYMBOL(cfpkt_tonative);
  79
  80static 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
  96inline struct cfpkt *cfpkt_create(u16 len)
  97{
  98        return cfpkt_create_pfx(len + PKT_POSTFIX, PKT_PREFIX);
  99}
 100
 101void cfpkt_destroy(struct cfpkt *pkt)
 102{
 103        struct sk_buff *skb = pkt_to_skb(pkt);
 104        kfree_skb(skb);
 105}
 106
 107inline bool cfpkt_more(struct cfpkt *pkt)
 108{
 109        struct sk_buff *skb = pkt_to_skb(pkt);
 110        return skb->len > 0;
 111}
 112
 113int 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
 124int 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}
 148EXPORT_SYMBOL(cfpkt_extr_head);
 149
 150int 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
 172int cfpkt_pad_trail(struct cfpkt *pkt, u16 len)
 173{
 174        return cfpkt_add_body(pkt, NULL, len);
 175}
 176
 177int 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
 225inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data)
 226{
 227        return cfpkt_add_body(pkt, &data, 1);
 228}
 229
 230int 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}
 255EXPORT_SYMBOL(cfpkt_add_head);
 256
 257inline int cfpkt_add_trail(struct cfpkt *pkt, const void *data, u16 len)
 258{
 259        return cfpkt_add_body(pkt, data, len);
 260}
 261
 262inline u16 cfpkt_getlen(struct cfpkt *pkt)
 263{
 264        struct sk_buff *skb = pkt_to_skb(pkt);
 265        return skb->len;
 266}
 267
 268inline 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
 285int 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
 309struct 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
 350struct 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
 388bool cfpkt_erroneous(struct cfpkt *pkt)
 389{
 390        return cfpkt_priv(pkt)->erronous;
 391}
 392
 393struct caif_payload_info *cfpkt_info(struct cfpkt *pkt)
 394{
 395        return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb;
 396}
 397EXPORT_SYMBOL(cfpkt_info);
 398
 399void cfpkt_set_prio(struct cfpkt *pkt, int prio)
 400{
 401        pkt_to_skb(pkt)->priority = prio;
 402}
 403EXPORT_SYMBOL(cfpkt_set_prio);
 404
lxr.linux.no kindly hosted by Redpill Linpro AS, provider of Linux consulting and operations services since 1995.