blob: d7e865e2ff651c13d21a1ad45d402a924864f0bf [file] [log] [blame]
Sjur Braendeland15c9ac02010-03-30 13:56:24 +00001/*
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
Joe Perchesb31fa5b2010-09-05 21:31:11 +00007#define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__
8
Sjur Braendeland15c9ac02010-03-30 13:56:24 +00009#include <linux/string.h>
10#include <linux/skbuff.h>
11#include <linux/hardirq.h>
12#include <net/caif/cfpkt.h>
13
Sjur Braendeland24e263a2010-08-10 07:36:46 +000014#define PKT_PREFIX 48
Sjur Braendeland2aa40ae2010-06-17 06:55:40 +000015#define PKT_POSTFIX 2
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000016#define PKT_LEN_WHEN_EXTENDING 128
Joe Perchesb31fa5b2010-09-05 21:31:11 +000017#define PKT_ERROR(pkt, errmsg) \
18do { \
19 cfpkt_priv(pkt)->erronous = true; \
20 skb_reset_tail_pointer(&pkt->skb); \
21 pr_warn(errmsg); \
22} while (0)
Sjur Braendeland15c9ac02010-03-30 13:56:24 +000023
24struct 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 */
35struct cfpkt {
36 struct sk_buff skb;
37};
38
39/* Private data inside SKB */
40struct cfpkt_priv_data {
41 struct dev_info dev_info;
42 bool erronous;
43};
44
45inline struct cfpkt_priv_data *cfpkt_priv(struct cfpkt *pkt)
46{
47 return (struct cfpkt_priv_data *) pkt->skb.cb;
48}
49
50inline bool is_erronous(struct cfpkt *pkt)
51{
52 return cfpkt_priv(pkt)->erronous;
53}
54
55inline struct sk_buff *pkt_to_skb(struct cfpkt *pkt)
56{
57 return &pkt->skb;
58}
59
60inline struct cfpkt *skb_to_pkt(struct sk_buff *skb)
61{
62 return (struct cfpkt *) skb;
63}
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}
100EXPORT_SYMBOL(cfpkt_create);
101
102void cfpkt_destroy(struct cfpkt *pkt)
103{
104 struct sk_buff *skb = pkt_to_skb(pkt);
105 kfree_skb(skb);
106}
107EXPORT_SYMBOL(cfpkt_destroy);
108
109inline bool cfpkt_more(struct cfpkt *pkt)
110{
111 struct sk_buff *skb = pkt_to_skb(pkt);
112 return skb->len > 0;
113}
114EXPORT_SYMBOL(cfpkt_more);
115
116int 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}
126EXPORT_SYMBOL(cfpkt_peek_head);
127
128int cfpkt_extr_head(struct cfpkt *pkt, void *data, u16 len)
129{
130 struct sk_buff *skb = pkt_to_skb(pkt);
131 u8 *from;
132 if (unlikely(is_erronous(pkt)))
133 return -EPROTO;
134
135 if (unlikely(len > skb->len)) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000136 PKT_ERROR(pkt, "read beyond end of packet\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000137 return -EPROTO;
138 }
139
140 if (unlikely(len > skb_headlen(skb))) {
141 if (unlikely(skb_linearize(skb) != 0)) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000142 PKT_ERROR(pkt, "linearize failed\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000143 return -EPROTO;
144 }
145 }
146 from = skb_pull(skb, len);
147 from -= len;
148 memcpy(data, from, len);
149 return 0;
150}
151EXPORT_SYMBOL(cfpkt_extr_head);
152
153int cfpkt_extr_trail(struct cfpkt *pkt, void *dta, u16 len)
154{
155 struct sk_buff *skb = pkt_to_skb(pkt);
156 u8 *data = dta;
157 u8 *from;
158 if (unlikely(is_erronous(pkt)))
159 return -EPROTO;
160
161 if (unlikely(skb_linearize(skb) != 0)) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000162 PKT_ERROR(pkt, "linearize failed\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000163 return -EPROTO;
164 }
165 if (unlikely(skb->data + len > skb_tail_pointer(skb))) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000166 PKT_ERROR(pkt, "read beyond end of packet\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000167 return -EPROTO;
168 }
169 from = skb_tail_pointer(skb) - len;
170 skb_trim(skb, skb->len - len);
171 memcpy(data, from, len);
172 return 0;
173}
174EXPORT_SYMBOL(cfpkt_extr_trail);
175
176int cfpkt_pad_trail(struct cfpkt *pkt, u16 len)
177{
178 return cfpkt_add_body(pkt, NULL, len);
179}
180EXPORT_SYMBOL(cfpkt_pad_trail);
181
182int cfpkt_add_body(struct cfpkt *pkt, const void *data, u16 len)
183{
184 struct sk_buff *skb = pkt_to_skb(pkt);
185 struct sk_buff *lastskb;
186 u8 *to;
187 u16 addlen = 0;
188
189
190 if (unlikely(is_erronous(pkt)))
191 return -EPROTO;
192
193 lastskb = skb;
194
195 /* Check whether we need to add space at the tail */
196 if (unlikely(skb_tailroom(skb) < len)) {
197 if (likely(len < PKT_LEN_WHEN_EXTENDING))
198 addlen = PKT_LEN_WHEN_EXTENDING;
199 else
200 addlen = len;
201 }
202
203 /* Check whether we need to change the SKB before writing to the tail */
204 if (unlikely((addlen > 0) || skb_cloned(skb) || skb_shared(skb))) {
205
206 /* Make sure data is writable */
207 if (unlikely(skb_cow_data(skb, addlen, &lastskb) < 0)) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000208 PKT_ERROR(pkt, "cow failed\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000209 return -EPROTO;
210 }
211 /*
212 * Is the SKB non-linear after skb_cow_data()? If so, we are
213 * going to add data to the last SKB, so we need to adjust
214 * lengths of the top SKB.
215 */
216 if (lastskb != skb) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000217 pr_warn("Packet is non-linear\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000218 skb->len += len;
219 skb->data_len += len;
220 }
221 }
222
223 /* All set to put the last SKB and optionally write data there. */
224 to = skb_put(lastskb, len);
225 if (likely(data))
226 memcpy(to, data, len);
227 return 0;
228}
229EXPORT_SYMBOL(cfpkt_add_body);
230
231inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data)
232{
233 return cfpkt_add_body(pkt, &data, 1);
234}
235EXPORT_SYMBOL(cfpkt_addbdy);
236
237int cfpkt_add_head(struct cfpkt *pkt, const void *data2, u16 len)
238{
239 struct sk_buff *skb = pkt_to_skb(pkt);
240 struct sk_buff *lastskb;
241 u8 *to;
242 const u8 *data = data2;
Sjur Braendeland638e6282010-05-21 02:16:09 +0000243 int ret;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000244 if (unlikely(is_erronous(pkt)))
245 return -EPROTO;
246 if (unlikely(skb_headroom(skb) < len)) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000247 PKT_ERROR(pkt, "no headroom\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000248 return -EPROTO;
249 }
250
251 /* Make sure data is writable */
Sjur Braendeland638e6282010-05-21 02:16:09 +0000252 ret = skb_cow_data(skb, 0, &lastskb);
253 if (unlikely(ret < 0)) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000254 PKT_ERROR(pkt, "cow failed\n");
Sjur Braendeland638e6282010-05-21 02:16:09 +0000255 return ret;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000256 }
257
258 to = skb_push(skb, len);
259 memcpy(to, data, len);
260 return 0;
261}
262EXPORT_SYMBOL(cfpkt_add_head);
263
264inline int cfpkt_add_trail(struct cfpkt *pkt, const void *data, u16 len)
265{
266 return cfpkt_add_body(pkt, data, len);
267}
268EXPORT_SYMBOL(cfpkt_add_trail);
269
270inline u16 cfpkt_getlen(struct cfpkt *pkt)
271{
272 struct sk_buff *skb = pkt_to_skb(pkt);
273 return skb->len;
274}
275EXPORT_SYMBOL(cfpkt_getlen);
276
277inline u16 cfpkt_iterate(struct cfpkt *pkt,
278 u16 (*iter_func)(u16, void *, u16),
279 u16 data)
280{
281 /*
282 * Don't care about the performance hit of linearizing,
283 * Checksum should not be used on high-speed interfaces anyway.
284 */
285 if (unlikely(is_erronous(pkt)))
286 return -EPROTO;
287 if (unlikely(skb_linearize(&pkt->skb) != 0)) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000288 PKT_ERROR(pkt, "linearize failed\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000289 return -EPROTO;
290 }
291 return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt));
292}
293EXPORT_SYMBOL(cfpkt_iterate);
294
295int cfpkt_setlen(struct cfpkt *pkt, u16 len)
296{
297 struct sk_buff *skb = pkt_to_skb(pkt);
298
299
300 if (unlikely(is_erronous(pkt)))
301 return -EPROTO;
302
303 if (likely(len <= skb->len)) {
304 if (unlikely(skb->data_len))
305 ___pskb_trim(skb, len);
306 else
307 skb_trim(skb, len);
308
309 return cfpkt_getlen(pkt);
310 }
311
312 /* Need to expand SKB */
313 if (unlikely(!cfpkt_pad_trail(pkt, len - skb->len)))
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000314 PKT_ERROR(pkt, "skb_pad_trail failed\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000315
316 return cfpkt_getlen(pkt);
317}
318EXPORT_SYMBOL(cfpkt_setlen);
319
320struct cfpkt *cfpkt_create_uplink(const unsigned char *data, unsigned int len)
321{
322 struct cfpkt *pkt = cfpkt_create_pfx(len + PKT_POSTFIX, PKT_PREFIX);
Sjur Braendeland638e6282010-05-21 02:16:09 +0000323 if (!pkt)
324 return NULL;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000325 if (unlikely(data != NULL))
326 cfpkt_add_body(pkt, data, len);
327 return pkt;
328}
329EXPORT_SYMBOL(cfpkt_create_uplink);
330
331struct cfpkt *cfpkt_append(struct cfpkt *dstpkt,
332 struct cfpkt *addpkt,
333 u16 expectlen)
334{
335 struct sk_buff *dst = pkt_to_skb(dstpkt);
336 struct sk_buff *add = pkt_to_skb(addpkt);
337 u16 addlen = skb_headlen(add);
338 u16 neededtailspace;
339 struct sk_buff *tmp;
340 u16 dstlen;
341 u16 createlen;
342 if (unlikely(is_erronous(dstpkt) || is_erronous(addpkt))) {
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000343 return dstpkt;
344 }
345 if (expectlen > addlen)
346 neededtailspace = expectlen;
347 else
348 neededtailspace = addlen;
349
350 if (dst->tail + neededtailspace > dst->end) {
351 /* Create a dumplicate of 'dst' with more tail space */
Sjur Braendeland638e6282010-05-21 02:16:09 +0000352 struct cfpkt *tmppkt;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000353 dstlen = skb_headlen(dst);
354 createlen = dstlen + neededtailspace;
Sjur Braendeland638e6282010-05-21 02:16:09 +0000355 tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX);
356 if (tmppkt == NULL)
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000357 return NULL;
Sjur Braendeland638e6282010-05-21 02:16:09 +0000358 tmp = pkt_to_skb(tmppkt);
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000359 skb_set_tail_pointer(tmp, dstlen);
360 tmp->len = dstlen;
361 memcpy(tmp->data, dst->data, dstlen);
362 cfpkt_destroy(dstpkt);
363 dst = tmp;
364 }
365 memcpy(skb_tail_pointer(dst), add->data, skb_headlen(add));
366 cfpkt_destroy(addpkt);
367 dst->tail += addlen;
368 dst->len += addlen;
369 return skb_to_pkt(dst);
370}
371EXPORT_SYMBOL(cfpkt_append);
372
373struct cfpkt *cfpkt_split(struct cfpkt *pkt, u16 pos)
374{
375 struct sk_buff *skb2;
376 struct sk_buff *skb = pkt_to_skb(pkt);
Sjur Braendeland638e6282010-05-21 02:16:09 +0000377 struct cfpkt *tmppkt;
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000378 u8 *split = skb->data + pos;
379 u16 len2nd = skb_tail_pointer(skb) - split;
380
381 if (unlikely(is_erronous(pkt)))
382 return NULL;
383
384 if (skb->data + pos > skb_tail_pointer(skb)) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000385 PKT_ERROR(pkt, "trying to split beyond end of packet\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000386 return NULL;
387 }
388
389 /* Create a new packet for the second part of the data */
Sjur Braendeland638e6282010-05-21 02:16:09 +0000390 tmppkt = cfpkt_create_pfx(len2nd + PKT_PREFIX + PKT_POSTFIX,
391 PKT_PREFIX);
392 if (tmppkt == NULL)
393 return NULL;
394 skb2 = pkt_to_skb(tmppkt);
395
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000396
397 if (skb2 == NULL)
398 return NULL;
399
400 /* Reduce the length of the original packet */
401 skb_set_tail_pointer(skb, pos);
402 skb->len = pos;
403
404 memcpy(skb2->data, split, len2nd);
405 skb2->tail += len2nd;
406 skb2->len += len2nd;
407 return skb_to_pkt(skb2);
408}
409EXPORT_SYMBOL(cfpkt_split);
410
411char *cfpkt_log_pkt(struct cfpkt *pkt, char *buf, int buflen)
412{
413 struct sk_buff *skb = pkt_to_skb(pkt);
414 char *p = buf;
415 int i;
416
417 /*
418 * Sanity check buffer length, it needs to be at least as large as
419 * the header info: ~=50+ bytes
420 */
421 if (buflen < 50)
422 return NULL;
423
424 snprintf(buf, buflen, "%s: pkt:%p len:%ld(%ld+%ld) {%ld,%ld} data: [",
425 is_erronous(pkt) ? "ERRONOUS-SKB" :
426 (skb->data_len != 0 ? "COMPLEX-SKB" : "SKB"),
427 skb,
428 (long) skb->len,
429 (long) (skb_tail_pointer(skb) - skb->data),
430 (long) skb->data_len,
431 (long) (skb->data - skb->head),
432 (long) (skb_tail_pointer(skb) - skb->head));
433 p = buf + strlen(buf);
434
435 for (i = 0; i < skb_tail_pointer(skb) - skb->data && i < 300; i++) {
436 if (p > buf + buflen - 10) {
437 sprintf(p, "...");
438 p = buf + strlen(buf);
439 break;
440 }
441 sprintf(p, "%02x,", skb->data[i]);
442 p = buf + strlen(buf);
443 }
444 sprintf(p, "]\n");
445 return buf;
446}
447EXPORT_SYMBOL(cfpkt_log_pkt);
448
449int cfpkt_raw_append(struct cfpkt *pkt, void **buf, unsigned int buflen)
450{
451 struct sk_buff *skb = pkt_to_skb(pkt);
452 struct sk_buff *lastskb;
453
454 caif_assert(buf != NULL);
455 if (unlikely(is_erronous(pkt)))
456 return -EPROTO;
457 /* Make sure SKB is writable */
458 if (unlikely(skb_cow_data(skb, 0, &lastskb) < 0)) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000459 PKT_ERROR(pkt, "skb_cow_data failed\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000460 return -EPROTO;
461 }
462
463 if (unlikely(skb_linearize(skb) != 0)) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000464 PKT_ERROR(pkt, "linearize failed\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000465 return -EPROTO;
466 }
467
468 if (unlikely(skb_tailroom(skb) < buflen)) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000469 PKT_ERROR(pkt, "buffer too short - failed\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000470 return -EPROTO;
471 }
472
473 *buf = skb_put(skb, buflen);
474 return 1;
475}
476EXPORT_SYMBOL(cfpkt_raw_append);
477
478int cfpkt_raw_extract(struct cfpkt *pkt, void **buf, unsigned int buflen)
479{
480 struct sk_buff *skb = pkt_to_skb(pkt);
481
482 caif_assert(buf != NULL);
483 if (unlikely(is_erronous(pkt)))
484 return -EPROTO;
485
486 if (unlikely(buflen > skb->len)) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000487 PKT_ERROR(pkt, "buflen too large - failed\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000488 return -EPROTO;
489 }
490
491 if (unlikely(buflen > skb_headlen(skb))) {
492 if (unlikely(skb_linearize(skb) != 0)) {
Joe Perchesb31fa5b2010-09-05 21:31:11 +0000493 PKT_ERROR(pkt, "linearize failed\n");
Sjur Braendeland15c9ac02010-03-30 13:56:24 +0000494 return -EPROTO;
495 }
496 }
497
498 *buf = skb->data;
499 skb_pull(skb, buflen);
500
501 return 1;
502}
503EXPORT_SYMBOL(cfpkt_raw_extract);
504
505inline bool cfpkt_erroneous(struct cfpkt *pkt)
506{
507 return cfpkt_priv(pkt)->erronous;
508}
509EXPORT_SYMBOL(cfpkt_erroneous);
510
511struct cfpktq *cfpktq_create(void)
512{
513 struct cfpktq *q = kmalloc(sizeof(struct cfpktq), GFP_ATOMIC);
514 if (!q)
515 return NULL;
516 skb_queue_head_init(&q->head);
517 atomic_set(&q->count, 0);
518 spin_lock_init(&q->lock);
519 return q;
520}
521EXPORT_SYMBOL(cfpktq_create);
522
523void cfpkt_queue(struct cfpktq *pktq, struct cfpkt *pkt, unsigned short prio)
524{
525 atomic_inc(&pktq->count);
526 spin_lock(&pktq->lock);
527 skb_queue_tail(&pktq->head, pkt_to_skb(pkt));
528 spin_unlock(&pktq->lock);
529
530}
531EXPORT_SYMBOL(cfpkt_queue);
532
533struct cfpkt *cfpkt_qpeek(struct cfpktq *pktq)
534{
535 struct cfpkt *tmp;
536 spin_lock(&pktq->lock);
537 tmp = skb_to_pkt(skb_peek(&pktq->head));
538 spin_unlock(&pktq->lock);
539 return tmp;
540}
541EXPORT_SYMBOL(cfpkt_qpeek);
542
543struct cfpkt *cfpkt_dequeue(struct cfpktq *pktq)
544{
545 struct cfpkt *pkt;
546 spin_lock(&pktq->lock);
547 pkt = skb_to_pkt(skb_dequeue(&pktq->head));
548 if (pkt) {
549 atomic_dec(&pktq->count);
550 caif_assert(atomic_read(&pktq->count) >= 0);
551 }
552 spin_unlock(&pktq->lock);
553 return pkt;
554}
555EXPORT_SYMBOL(cfpkt_dequeue);
556
557int cfpkt_qcount(struct cfpktq *pktq)
558{
559 return atomic_read(&pktq->count);
560}
561EXPORT_SYMBOL(cfpkt_qcount);
562
563struct cfpkt *cfpkt_clone_release(struct cfpkt *pkt)
564{
565 struct cfpkt *clone;
566 clone = skb_to_pkt(skb_clone(pkt_to_skb(pkt), GFP_ATOMIC));
567 /* Free original packet. */
568 cfpkt_destroy(pkt);
569 if (!clone)
570 return NULL;
571 return clone;
572}
573EXPORT_SYMBOL(cfpkt_clone_release);
574
575struct caif_payload_info *cfpkt_info(struct cfpkt *pkt)
576{
577 return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb;
578}
579EXPORT_SYMBOL(cfpkt_info);