2 * Copyright (C) ST-Ericsson AB 2010
3 * Author: Sjur Brendeland
4 * License terms: GNU General Public License (GPL) version 2
7 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
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>
17 #define PKT_LEN_WHEN_EXTENDING 128
18 #define PKT_ERROR(pkt, errmsg) \
20 cfpkt_priv(pkt)->erronous = true; \
21 skb_reset_tail_pointer(&pkt->skb); \
26 struct sk_buff_head head
;
28 /* Lock protects count updates */
33 * net/caif/ is generic and does not
34 * understand SKB, so we do this typecast
40 /* Private data inside SKB */
41 struct cfpkt_priv_data
{
42 struct dev_info dev_info
;
46 static inline struct cfpkt_priv_data
*cfpkt_priv(struct cfpkt
*pkt
)
48 return (struct cfpkt_priv_data
*) pkt
->skb
.cb
;
51 static inline bool is_erronous(struct cfpkt
*pkt
)
53 return cfpkt_priv(pkt
)->erronous
;
56 static inline struct sk_buff
*pkt_to_skb(struct cfpkt
*pkt
)
61 static inline struct cfpkt
*skb_to_pkt(struct sk_buff
*skb
)
63 return (struct cfpkt
*) skb
;
66 struct cfpkt
*cfpkt_fromnative(enum caif_direction dir
, void *nativepkt
)
68 struct cfpkt
*pkt
= skb_to_pkt(nativepkt
);
69 cfpkt_priv(pkt
)->erronous
= false;
72 EXPORT_SYMBOL(cfpkt_fromnative
);
74 void *cfpkt_tonative(struct cfpkt
*pkt
)
78 EXPORT_SYMBOL(cfpkt_tonative
);
80 static struct cfpkt
*cfpkt_create_pfx(u16 len
, u16 pfx
)
84 if (likely(in_interrupt()))
85 skb
= alloc_skb(len
+ pfx
, GFP_ATOMIC
);
87 skb
= alloc_skb(len
+ pfx
, GFP_KERNEL
);
89 if (unlikely(skb
== NULL
))
92 skb_reserve(skb
, pfx
);
93 return skb_to_pkt(skb
);
96 inline struct cfpkt
*cfpkt_create(u16 len
)
98 return cfpkt_create_pfx(len
+ PKT_POSTFIX
, PKT_PREFIX
);
101 void cfpkt_destroy(struct cfpkt
*pkt
)
103 struct sk_buff
*skb
= pkt_to_skb(pkt
);
107 inline bool cfpkt_more(struct cfpkt
*pkt
)
109 struct sk_buff
*skb
= pkt_to_skb(pkt
);
113 int cfpkt_peek_head(struct cfpkt
*pkt
, void *data
, u16 len
)
115 struct sk_buff
*skb
= pkt_to_skb(pkt
);
116 if (skb_headlen(skb
) >= len
) {
117 memcpy(data
, skb
->data
, len
);
120 return !cfpkt_extr_head(pkt
, data
, len
) &&
121 !cfpkt_add_head(pkt
, data
, len
);
124 int cfpkt_extr_head(struct cfpkt
*pkt
, void *data
, u16 len
)
126 struct sk_buff
*skb
= pkt_to_skb(pkt
);
128 if (unlikely(is_erronous(pkt
)))
131 if (unlikely(len
> skb
->len
)) {
132 PKT_ERROR(pkt
, "read beyond end of packet\n");
136 if (unlikely(len
> skb_headlen(skb
))) {
137 if (unlikely(skb_linearize(skb
) != 0)) {
138 PKT_ERROR(pkt
, "linearize failed\n");
142 from
= skb_pull(skb
, len
);
145 memcpy(data
, from
, len
);
148 EXPORT_SYMBOL(cfpkt_extr_head
);
150 int cfpkt_extr_trail(struct cfpkt
*pkt
, void *dta
, u16 len
)
152 struct sk_buff
*skb
= pkt_to_skb(pkt
);
155 if (unlikely(is_erronous(pkt
)))
158 if (unlikely(skb_linearize(skb
) != 0)) {
159 PKT_ERROR(pkt
, "linearize failed\n");
162 if (unlikely(skb
->data
+ len
> skb_tail_pointer(skb
))) {
163 PKT_ERROR(pkt
, "read beyond end of packet\n");
166 from
= skb_tail_pointer(skb
) - len
;
167 skb_trim(skb
, skb
->len
- len
);
168 memcpy(data
, from
, len
);
172 int cfpkt_pad_trail(struct cfpkt
*pkt
, u16 len
)
174 return cfpkt_add_body(pkt
, NULL
, len
);
177 int cfpkt_add_body(struct cfpkt
*pkt
, const void *data
, u16 len
)
179 struct sk_buff
*skb
= pkt_to_skb(pkt
);
180 struct sk_buff
*lastskb
;
185 if (unlikely(is_erronous(pkt
)))
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
;
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
))) {
201 /* Make sure data is writable */
202 if (unlikely(skb_cow_data(skb
, addlen
, &lastskb
) < 0)) {
203 PKT_ERROR(pkt
, "cow failed\n");
208 /* All set to put the last SKB and optionally write data there. */
209 to
= pskb_put(skb
, lastskb
, len
);
211 memcpy(to
, data
, len
);
215 inline int cfpkt_addbdy(struct cfpkt
*pkt
, u8 data
)
217 return cfpkt_add_body(pkt
, &data
, 1);
220 int cfpkt_add_head(struct cfpkt
*pkt
, const void *data2
, u16 len
)
222 struct sk_buff
*skb
= pkt_to_skb(pkt
);
223 struct sk_buff
*lastskb
;
225 const u8
*data
= data2
;
227 if (unlikely(is_erronous(pkt
)))
229 if (unlikely(skb_headroom(skb
) < len
)) {
230 PKT_ERROR(pkt
, "no headroom\n");
234 /* Make sure data is writable */
235 ret
= skb_cow_data(skb
, 0, &lastskb
);
236 if (unlikely(ret
< 0)) {
237 PKT_ERROR(pkt
, "cow failed\n");
241 to
= skb_push(skb
, len
);
242 memcpy(to
, data
, len
);
245 EXPORT_SYMBOL(cfpkt_add_head
);
247 inline int cfpkt_add_trail(struct cfpkt
*pkt
, const void *data
, u16 len
)
249 return cfpkt_add_body(pkt
, data
, len
);
252 inline u16
cfpkt_getlen(struct cfpkt
*pkt
)
254 struct sk_buff
*skb
= pkt_to_skb(pkt
);
258 inline u16
cfpkt_iterate(struct cfpkt
*pkt
,
259 u16 (*iter_func
)(u16
, void *, u16
),
263 * Don't care about the performance hit of linearizing,
264 * Checksum should not be used on high-speed interfaces anyway.
266 if (unlikely(is_erronous(pkt
)))
268 if (unlikely(skb_linearize(&pkt
->skb
) != 0)) {
269 PKT_ERROR(pkt
, "linearize failed\n");
272 return iter_func(data
, pkt
->skb
.data
, cfpkt_getlen(pkt
));
275 int cfpkt_setlen(struct cfpkt
*pkt
, u16 len
)
277 struct sk_buff
*skb
= pkt_to_skb(pkt
);
280 if (unlikely(is_erronous(pkt
)))
283 if (likely(len
<= skb
->len
)) {
284 if (unlikely(skb
->data_len
))
285 ___pskb_trim(skb
, len
);
289 return cfpkt_getlen(pkt
);
292 /* Need to expand SKB */
293 if (unlikely(!cfpkt_pad_trail(pkt
, len
- skb
->len
)))
294 PKT_ERROR(pkt
, "skb_pad_trail failed\n");
296 return cfpkt_getlen(pkt
);
299 struct cfpkt
*cfpkt_append(struct cfpkt
*dstpkt
,
300 struct cfpkt
*addpkt
,
303 struct sk_buff
*dst
= pkt_to_skb(dstpkt
);
304 struct sk_buff
*add
= pkt_to_skb(addpkt
);
305 u16 addlen
= skb_headlen(add
);
310 if (unlikely(is_erronous(dstpkt
) || is_erronous(addpkt
))) {
313 if (expectlen
> addlen
)
314 neededtailspace
= expectlen
;
316 neededtailspace
= addlen
;
318 if (dst
->tail
+ neededtailspace
> dst
->end
) {
319 /* Create a dumplicate of 'dst' with more tail space */
320 struct cfpkt
*tmppkt
;
321 dstlen
= skb_headlen(dst
);
322 createlen
= dstlen
+ neededtailspace
;
323 tmppkt
= cfpkt_create(createlen
+ PKT_PREFIX
+ PKT_POSTFIX
);
326 tmp
= pkt_to_skb(tmppkt
);
327 skb_set_tail_pointer(tmp
, dstlen
);
329 memcpy(tmp
->data
, dst
->data
, dstlen
);
330 cfpkt_destroy(dstpkt
);
333 memcpy(skb_tail_pointer(dst
), add
->data
, skb_headlen(add
));
334 cfpkt_destroy(addpkt
);
337 return skb_to_pkt(dst
);
340 struct cfpkt
*cfpkt_split(struct cfpkt
*pkt
, u16 pos
)
342 struct sk_buff
*skb2
;
343 struct sk_buff
*skb
= pkt_to_skb(pkt
);
344 struct cfpkt
*tmppkt
;
345 u8
*split
= skb
->data
+ pos
;
346 u16 len2nd
= skb_tail_pointer(skb
) - split
;
348 if (unlikely(is_erronous(pkt
)))
351 if (skb
->data
+ pos
> skb_tail_pointer(skb
)) {
352 PKT_ERROR(pkt
, "trying to split beyond end of packet\n");
356 /* Create a new packet for the second part of the data */
357 tmppkt
= cfpkt_create_pfx(len2nd
+ PKT_PREFIX
+ PKT_POSTFIX
,
361 skb2
= pkt_to_skb(tmppkt
);
367 /* Reduce the length of the original packet */
368 skb_set_tail_pointer(skb
, pos
);
371 memcpy(skb2
->data
, split
, len2nd
);
372 skb2
->tail
+= len2nd
;
374 skb2
->priority
= skb
->priority
;
375 return skb_to_pkt(skb2
);
378 bool cfpkt_erroneous(struct cfpkt
*pkt
)
380 return cfpkt_priv(pkt
)->erronous
;
383 struct caif_payload_info
*cfpkt_info(struct cfpkt
*pkt
)
385 return (struct caif_payload_info
*)&pkt_to_skb(pkt
)->cb
;
387 EXPORT_SYMBOL(cfpkt_info
);
389 void cfpkt_set_prio(struct cfpkt
*pkt
, int prio
)
391 pkt_to_skb(pkt
)->priority
= prio
;
393 EXPORT_SYMBOL(cfpkt_set_prio
);
This page took 0.039411 seconds and 6 git commands to generate.