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