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