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
7 #include <linux/string.h>
8 #include <linux/skbuff.h>
9 #include <linux/hardirq.h>
10 #include <net/caif/cfpkt.h>
12 #define PKT_PREFIX CAIF_NEEDED_HEADROOM
13 #define PKT_POSTFIX CAIF_NEEDED_TAILROOM
14 #define PKT_LEN_WHEN_EXTENDING 128
15 #define PKT_ERROR(pkt, errmsg) do { \
16 cfpkt_priv(pkt)->erronous = true; \
17 skb_reset_tail_pointer(&pkt->skb); \
18 pr_warning("CAIF: " errmsg);\
22 struct sk_buff_head head
;
24 /* Lock protects count updates */
29 * net/caif/ is generic and does not
30 * understand SKB, so we do this typecast
36 /* Private data inside SKB */
37 struct cfpkt_priv_data
{
38 struct dev_info dev_info
;
42 inline struct cfpkt_priv_data
*cfpkt_priv(struct cfpkt
*pkt
)
44 return (struct cfpkt_priv_data
*) pkt
->skb
.cb
;
47 inline bool is_erronous(struct cfpkt
*pkt
)
49 return cfpkt_priv(pkt
)->erronous
;
52 inline struct sk_buff
*pkt_to_skb(struct cfpkt
*pkt
)
57 inline struct cfpkt
*skb_to_pkt(struct sk_buff
*skb
)
59 return (struct cfpkt
*) skb
;
63 struct cfpkt
*cfpkt_fromnative(enum caif_direction dir
, void *nativepkt
)
65 struct cfpkt
*pkt
= skb_to_pkt(nativepkt
);
66 cfpkt_priv(pkt
)->erronous
= false;
69 EXPORT_SYMBOL(cfpkt_fromnative
);
71 void *cfpkt_tonative(struct cfpkt
*pkt
)
75 EXPORT_SYMBOL(cfpkt_tonative
);
77 static struct cfpkt
*cfpkt_create_pfx(u16 len
, u16 pfx
)
81 if (likely(in_interrupt()))
82 skb
= alloc_skb(len
+ pfx
, GFP_ATOMIC
);
84 skb
= alloc_skb(len
+ pfx
, GFP_KERNEL
);
86 if (unlikely(skb
== NULL
))
89 skb_reserve(skb
, pfx
);
90 return skb_to_pkt(skb
);
93 inline struct cfpkt
*cfpkt_create(u16 len
)
95 return cfpkt_create_pfx(len
+ PKT_POSTFIX
, PKT_PREFIX
);
97 EXPORT_SYMBOL(cfpkt_create
);
99 void cfpkt_destroy(struct cfpkt
*pkt
)
101 struct sk_buff
*skb
= pkt_to_skb(pkt
);
104 EXPORT_SYMBOL(cfpkt_destroy
);
106 inline bool cfpkt_more(struct cfpkt
*pkt
)
108 struct sk_buff
*skb
= pkt_to_skb(pkt
);
111 EXPORT_SYMBOL(cfpkt_more
);
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
);
123 EXPORT_SYMBOL(cfpkt_peek_head
);
125 int cfpkt_extr_head(struct cfpkt
*pkt
, void *data
, u16 len
)
127 struct sk_buff
*skb
= pkt_to_skb(pkt
);
129 if (unlikely(is_erronous(pkt
)))
132 if (unlikely(len
> skb
->len
)) {
133 PKT_ERROR(pkt
, "cfpkt_extr_head read beyond end of packet\n");
137 if (unlikely(len
> skb_headlen(skb
))) {
138 if (unlikely(skb_linearize(skb
) != 0)) {
139 PKT_ERROR(pkt
, "cfpkt_extr_head linearize failed\n");
143 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
, "cfpkt_extr_trail linearize failed\n");
162 if (unlikely(skb
->data
+ len
> skb_tail_pointer(skb
))) {
163 PKT_ERROR(pkt
, "cfpkt_extr_trail 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
);
171 EXPORT_SYMBOL(cfpkt_extr_trail
);
173 int cfpkt_pad_trail(struct cfpkt
*pkt
, u16 len
)
175 return cfpkt_add_body(pkt
, NULL
, len
);
177 EXPORT_SYMBOL(cfpkt_pad_trail
);
179 int cfpkt_add_body(struct cfpkt
*pkt
, const void *data
, u16 len
)
181 struct sk_buff
*skb
= pkt_to_skb(pkt
);
182 struct sk_buff
*lastskb
;
187 if (unlikely(is_erronous(pkt
)))
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
;
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
))) {
203 /* Make sure data is writable */
204 if (unlikely(skb_cow_data(skb
, addlen
, &lastskb
) < 0)) {
205 PKT_ERROR(pkt
, "cfpkt_add_body: cow failed\n");
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.
213 if (lastskb
!= skb
) {
214 pr_warning("CAIF: %s(): Packet is non-linear\n",
217 skb
->data_len
+= len
;
221 /* All set to put the last SKB and optionally write data there. */
222 to
= skb_put(lastskb
, len
);
224 memcpy(to
, data
, len
);
227 EXPORT_SYMBOL(cfpkt_add_body
);
229 inline int cfpkt_addbdy(struct cfpkt
*pkt
, u8 data
)
231 return cfpkt_add_body(pkt
, &data
, 1);
233 EXPORT_SYMBOL(cfpkt_addbdy
);
235 int cfpkt_add_head(struct cfpkt
*pkt
, const void *data2
, u16 len
)
237 struct sk_buff
*skb
= pkt_to_skb(pkt
);
238 struct sk_buff
*lastskb
;
240 const u8
*data
= data2
;
241 if (unlikely(is_erronous(pkt
)))
243 if (unlikely(skb_headroom(skb
) < len
)) {
244 PKT_ERROR(pkt
, "cfpkt_add_head: no headroom\n");
248 /* Make sure data is writable */
249 if (unlikely(skb_cow_data(skb
, 0, &lastskb
) < 0)) {
250 PKT_ERROR(pkt
, "cfpkt_add_head: cow failed\n");
254 to
= skb_push(skb
, len
);
255 memcpy(to
, data
, len
);
258 EXPORT_SYMBOL(cfpkt_add_head
);
260 inline int cfpkt_add_trail(struct cfpkt
*pkt
, const void *data
, u16 len
)
262 return cfpkt_add_body(pkt
, data
, len
);
264 EXPORT_SYMBOL(cfpkt_add_trail
);
266 inline u16
cfpkt_getlen(struct cfpkt
*pkt
)
268 struct sk_buff
*skb
= pkt_to_skb(pkt
);
271 EXPORT_SYMBOL(cfpkt_getlen
);
273 inline u16
cfpkt_iterate(struct cfpkt
*pkt
,
274 u16 (*iter_func
)(u16
, void *, u16
),
278 * Don't care about the performance hit of linearizing,
279 * Checksum should not be used on high-speed interfaces anyway.
281 if (unlikely(is_erronous(pkt
)))
283 if (unlikely(skb_linearize(&pkt
->skb
) != 0)) {
284 PKT_ERROR(pkt
, "cfpkt_iterate: linearize failed\n");
287 return iter_func(data
, pkt
->skb
.data
, cfpkt_getlen(pkt
));
289 EXPORT_SYMBOL(cfpkt_iterate
);
291 int cfpkt_setlen(struct cfpkt
*pkt
, u16 len
)
293 struct sk_buff
*skb
= pkt_to_skb(pkt
);
296 if (unlikely(is_erronous(pkt
)))
299 if (likely(len
<= skb
->len
)) {
300 if (unlikely(skb
->data_len
))
301 ___pskb_trim(skb
, len
);
305 return cfpkt_getlen(pkt
);
308 /* Need to expand SKB */
309 if (unlikely(!cfpkt_pad_trail(pkt
, len
- skb
->len
)))
310 PKT_ERROR(pkt
, "cfpkt_setlen: skb_pad_trail failed\n");
312 return cfpkt_getlen(pkt
);
314 EXPORT_SYMBOL(cfpkt_setlen
);
316 struct cfpkt
*cfpkt_create_uplink(const unsigned char *data
, unsigned int len
)
318 struct cfpkt
*pkt
= cfpkt_create_pfx(len
+ PKT_POSTFIX
, PKT_PREFIX
);
319 if (unlikely(data
!= NULL
))
320 cfpkt_add_body(pkt
, data
, len
);
323 EXPORT_SYMBOL(cfpkt_create_uplink
);
325 struct cfpkt
*cfpkt_append(struct cfpkt
*dstpkt
,
326 struct cfpkt
*addpkt
,
329 struct sk_buff
*dst
= pkt_to_skb(dstpkt
);
330 struct sk_buff
*add
= pkt_to_skb(addpkt
);
331 u16 addlen
= skb_headlen(add
);
336 if (unlikely(is_erronous(dstpkt
) || is_erronous(addpkt
))) {
337 cfpkt_destroy(addpkt
);
340 if (expectlen
> addlen
)
341 neededtailspace
= expectlen
;
343 neededtailspace
= addlen
;
345 if (dst
->tail
+ neededtailspace
> dst
->end
) {
346 /* Create a dumplicate of 'dst' with more tail space */
347 dstlen
= skb_headlen(dst
);
348 createlen
= dstlen
+ neededtailspace
;
350 cfpkt_create(createlen
+ PKT_PREFIX
+ PKT_POSTFIX
));
353 skb_set_tail_pointer(tmp
, dstlen
);
355 memcpy(tmp
->data
, dst
->data
, dstlen
);
356 cfpkt_destroy(dstpkt
);
359 memcpy(skb_tail_pointer(dst
), add
->data
, skb_headlen(add
));
360 cfpkt_destroy(addpkt
);
363 return skb_to_pkt(dst
);
365 EXPORT_SYMBOL(cfpkt_append
);
367 struct cfpkt
*cfpkt_split(struct cfpkt
*pkt
, u16 pos
)
369 struct sk_buff
*skb2
;
370 struct sk_buff
*skb
= pkt_to_skb(pkt
);
371 u8
*split
= skb
->data
+ pos
;
372 u16 len2nd
= skb_tail_pointer(skb
) - split
;
374 if (unlikely(is_erronous(pkt
)))
377 if (skb
->data
+ pos
> skb_tail_pointer(skb
)) {
379 "cfpkt_split: trying to split beyond end of packet");
383 /* Create a new packet for the second part of the data */
385 cfpkt_create_pfx(len2nd
+ PKT_PREFIX
+ PKT_POSTFIX
,
391 /* Reduce the length of the original packet */
392 skb_set_tail_pointer(skb
, pos
);
395 memcpy(skb2
->data
, split
, len2nd
);
396 skb2
->tail
+= len2nd
;
398 return skb_to_pkt(skb2
);
400 EXPORT_SYMBOL(cfpkt_split
);
402 char *cfpkt_log_pkt(struct cfpkt
*pkt
, char *buf
, int buflen
)
404 struct sk_buff
*skb
= pkt_to_skb(pkt
);
409 * Sanity check buffer length, it needs to be at least as large as
410 * the header info: ~=50+ bytes
415 snprintf(buf
, buflen
, "%s: pkt:%p len:%ld(%ld+%ld) {%ld,%ld} data: [",
416 is_erronous(pkt
) ? "ERRONOUS-SKB" :
417 (skb
->data_len
!= 0 ? "COMPLEX-SKB" : "SKB"),
420 (long) (skb_tail_pointer(skb
) - skb
->data
),
421 (long) skb
->data_len
,
422 (long) (skb
->data
- skb
->head
),
423 (long) (skb_tail_pointer(skb
) - skb
->head
));
424 p
= buf
+ strlen(buf
);
426 for (i
= 0; i
< skb_tail_pointer(skb
) - skb
->data
&& i
< 300; i
++) {
427 if (p
> buf
+ buflen
- 10) {
429 p
= buf
+ strlen(buf
);
432 sprintf(p
, "%02x,", skb
->data
[i
]);
433 p
= buf
+ strlen(buf
);
438 EXPORT_SYMBOL(cfpkt_log_pkt
);
440 int cfpkt_raw_append(struct cfpkt
*pkt
, void **buf
, unsigned int buflen
)
442 struct sk_buff
*skb
= pkt_to_skb(pkt
);
443 struct sk_buff
*lastskb
;
445 caif_assert(buf
!= NULL
);
446 if (unlikely(is_erronous(pkt
)))
448 /* Make sure SKB is writable */
449 if (unlikely(skb_cow_data(skb
, 0, &lastskb
) < 0)) {
450 PKT_ERROR(pkt
, "cfpkt_raw_append: skb_cow_data failed\n");
454 if (unlikely(skb_linearize(skb
) != 0)) {
455 PKT_ERROR(pkt
, "cfpkt_raw_append: linearize failed\n");
459 if (unlikely(skb_tailroom(skb
) < buflen
)) {
460 PKT_ERROR(pkt
, "cfpkt_raw_append: buffer too short - failed\n");
464 *buf
= skb_put(skb
, buflen
);
467 EXPORT_SYMBOL(cfpkt_raw_append
);
469 int cfpkt_raw_extract(struct cfpkt
*pkt
, void **buf
, unsigned int buflen
)
471 struct sk_buff
*skb
= pkt_to_skb(pkt
);
473 caif_assert(buf
!= NULL
);
474 if (unlikely(is_erronous(pkt
)))
477 if (unlikely(buflen
> skb
->len
)) {
478 PKT_ERROR(pkt
, "cfpkt_raw_extract: buflen too large "
483 if (unlikely(buflen
> skb_headlen(skb
))) {
484 if (unlikely(skb_linearize(skb
) != 0)) {
485 PKT_ERROR(pkt
, "cfpkt_raw_extract: linearize failed\n");
491 skb_pull(skb
, buflen
);
495 EXPORT_SYMBOL(cfpkt_raw_extract
);
497 inline bool cfpkt_erroneous(struct cfpkt
*pkt
)
499 return cfpkt_priv(pkt
)->erronous
;
501 EXPORT_SYMBOL(cfpkt_erroneous
);
503 struct cfpktq
*cfpktq_create(void)
505 struct cfpktq
*q
= kmalloc(sizeof(struct cfpktq
), GFP_ATOMIC
);
508 skb_queue_head_init(&q
->head
);
509 atomic_set(&q
->count
, 0);
510 spin_lock_init(&q
->lock
);
513 EXPORT_SYMBOL(cfpktq_create
);
515 void cfpkt_queue(struct cfpktq
*pktq
, struct cfpkt
*pkt
, unsigned short prio
)
517 atomic_inc(&pktq
->count
);
518 spin_lock(&pktq
->lock
);
519 skb_queue_tail(&pktq
->head
, pkt_to_skb(pkt
));
520 spin_unlock(&pktq
->lock
);
523 EXPORT_SYMBOL(cfpkt_queue
);
525 struct cfpkt
*cfpkt_qpeek(struct cfpktq
*pktq
)
528 spin_lock(&pktq
->lock
);
529 tmp
= skb_to_pkt(skb_peek(&pktq
->head
));
530 spin_unlock(&pktq
->lock
);
533 EXPORT_SYMBOL(cfpkt_qpeek
);
535 struct cfpkt
*cfpkt_dequeue(struct cfpktq
*pktq
)
538 spin_lock(&pktq
->lock
);
539 pkt
= skb_to_pkt(skb_dequeue(&pktq
->head
));
541 atomic_dec(&pktq
->count
);
542 caif_assert(atomic_read(&pktq
->count
) >= 0);
544 spin_unlock(&pktq
->lock
);
547 EXPORT_SYMBOL(cfpkt_dequeue
);
549 int cfpkt_qcount(struct cfpktq
*pktq
)
551 return atomic_read(&pktq
->count
);
553 EXPORT_SYMBOL(cfpkt_qcount
);
555 struct cfpkt
*cfpkt_clone_release(struct cfpkt
*pkt
)
558 clone
= skb_to_pkt(skb_clone(pkt_to_skb(pkt
), GFP_ATOMIC
));
559 /* Free original packet. */
565 EXPORT_SYMBOL(cfpkt_clone_release
);
567 struct caif_payload_info
*cfpkt_info(struct cfpkt
*pkt
)
569 return (struct caif_payload_info
*)&pkt_to_skb(pkt
)->cb
;
571 EXPORT_SYMBOL(cfpkt_info
);