Commit | Line | Data |
---|---|---|
15c9ac0c SB |
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 | ||
b31fa5ba JP |
7 | #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ |
8 | ||
15c9ac0c SB |
9 | #include <linux/string.h> |
10 | #include <linux/skbuff.h> | |
11 | #include <linux/hardirq.h> | |
bc3b2d7f | 12 | #include <linux/export.h> |
15c9ac0c SB |
13 | #include <net/caif/cfpkt.h> |
14 | ||
24e263ad | 15 | #define PKT_PREFIX 48 |
2aa40aef | 16 | #define PKT_POSTFIX 2 |
15c9ac0c | 17 | #define PKT_LEN_WHEN_EXTENDING 128 |
b31fa5ba JP |
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) | |
15c9ac0c SB |
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 | ||
73d6ac63 | 46 | static inline struct cfpkt_priv_data *cfpkt_priv(struct cfpkt *pkt) |
15c9ac0c SB |
47 | { |
48 | return (struct cfpkt_priv_data *) pkt->skb.cb; | |
49 | } | |
50 | ||
73d6ac63 | 51 | static inline bool is_erronous(struct cfpkt *pkt) |
15c9ac0c SB |
52 | { |
53 | return cfpkt_priv(pkt)->erronous; | |
54 | } | |
55 | ||
73d6ac63 | 56 | static inline struct sk_buff *pkt_to_skb(struct cfpkt *pkt) |
15c9ac0c SB |
57 | { |
58 | return &pkt->skb; | |
59 | } | |
60 | ||
73d6ac63 | 61 | static inline struct cfpkt *skb_to_pkt(struct sk_buff *skb) |
15c9ac0c SB |
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 | } | |
15c9ac0c SB |
101 | |
102 | void cfpkt_destroy(struct cfpkt *pkt) | |
103 | { | |
104 | struct sk_buff *skb = pkt_to_skb(pkt); | |
105 | kfree_skb(skb); | |
106 | } | |
3f874adc | 107 | |
15c9ac0c SB |
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 | } | |
3f874adc | 114 | |
15c9ac0c SB |
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 | } | |
15c9ac0c SB |
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)) { | |
b31fa5ba | 135 | PKT_ERROR(pkt, "read beyond end of packet\n"); |
15c9ac0c SB |
136 | return -EPROTO; |
137 | } | |
138 | ||
139 | if (unlikely(len > skb_headlen(skb))) { | |
140 | if (unlikely(skb_linearize(skb) != 0)) { | |
b31fa5ba | 141 | PKT_ERROR(pkt, "linearize failed\n"); |
15c9ac0c SB |
142 | return -EPROTO; |
143 | } | |
144 | } | |
145 | from = skb_pull(skb, len); | |
146 | from -= len; | |
200c5a3b | 147 | if (data) |
148 | memcpy(data, from, len); | |
15c9ac0c SB |
149 | return 0; |
150 | } | |
15c9ac0c SB |
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)) { | |
b31fa5ba | 161 | PKT_ERROR(pkt, "linearize failed\n"); |
15c9ac0c SB |
162 | return -EPROTO; |
163 | } | |
164 | if (unlikely(skb->data + len > skb_tail_pointer(skb))) { | |
b31fa5ba | 165 | PKT_ERROR(pkt, "read beyond end of packet\n"); |
15c9ac0c SB |
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 | } | |
3f874adc | 173 | |
15c9ac0c SB |
174 | |
175 | int cfpkt_pad_trail(struct cfpkt *pkt, u16 len) | |
176 | { | |
177 | return cfpkt_add_body(pkt, NULL, len); | |
178 | } | |
3f874adc | 179 | |
15c9ac0c SB |
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)) { | |
b31fa5ba | 207 | PKT_ERROR(pkt, "cow failed\n"); |
15c9ac0c SB |
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) { | |
b31fa5ba | 216 | pr_warn("Packet is non-linear\n"); |
15c9ac0c SB |
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 | } | |
15c9ac0c SB |
228 | |
229 | inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data) | |
230 | { | |
231 | return cfpkt_add_body(pkt, &data, 1); | |
232 | } | |
15c9ac0c SB |
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; | |
638e628a | 240 | int ret; |
15c9ac0c SB |
241 | if (unlikely(is_erronous(pkt))) |
242 | return -EPROTO; | |
243 | if (unlikely(skb_headroom(skb) < len)) { | |
b31fa5ba | 244 | PKT_ERROR(pkt, "no headroom\n"); |
15c9ac0c SB |
245 | return -EPROTO; |
246 | } | |
247 | ||
248 | /* Make sure data is writable */ | |
638e628a SB |
249 | ret = skb_cow_data(skb, 0, &lastskb); |
250 | if (unlikely(ret < 0)) { | |
b31fa5ba | 251 | PKT_ERROR(pkt, "cow failed\n"); |
638e628a | 252 | return ret; |
15c9ac0c SB |
253 | } |
254 | ||
255 | to = skb_push(skb, len); | |
256 | memcpy(to, data, len); | |
257 | return 0; | |
258 | } | |
3f874adc | 259 | |
15c9ac0c SB |
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 | } | |
3f874adc | 265 | |
15c9ac0c SB |
266 | |
267 | inline u16 cfpkt_getlen(struct cfpkt *pkt) | |
268 | { | |
269 | struct sk_buff *skb = pkt_to_skb(pkt); | |
270 | return skb->len; | |
271 | } | |
3f874adc | 272 | |
15c9ac0c SB |
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)) { | |
b31fa5ba | 285 | PKT_ERROR(pkt, "linearize failed\n"); |
15c9ac0c SB |
286 | return -EPROTO; |
287 | } | |
288 | return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt)); | |
289 | } | |
3f874adc | 290 | |
15c9ac0c SB |
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))) | |
b31fa5ba | 311 | PKT_ERROR(pkt, "skb_pad_trail failed\n"); |
15c9ac0c SB |
312 | |
313 | return cfpkt_getlen(pkt); | |
314 | } | |
15c9ac0c | 315 | |
15c9ac0c SB |
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))) { | |
15c9ac0c SB |
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 */ | |
638e628a | 337 | struct cfpkt *tmppkt; |
15c9ac0c SB |
338 | dstlen = skb_headlen(dst); |
339 | createlen = dstlen + neededtailspace; | |
638e628a SB |
340 | tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX); |
341 | if (tmppkt == NULL) | |
15c9ac0c | 342 | return NULL; |
638e628a | 343 | tmp = pkt_to_skb(tmppkt); |
15c9ac0c SB |
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 | } | |
15c9ac0c SB |
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); | |
638e628a | 361 | struct cfpkt *tmppkt; |
15c9ac0c SB |
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)) { | |
b31fa5ba | 369 | PKT_ERROR(pkt, "trying to split beyond end of packet\n"); |
15c9ac0c SB |
370 | return NULL; |
371 | } | |
372 | ||
373 | /* Create a new packet for the second part of the data */ | |
638e628a SB |
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 | ||
15c9ac0c SB |
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 | } | |
15c9ac0c | 393 | |
73d6ac63 | 394 | bool cfpkt_erroneous(struct cfpkt *pkt) |
15c9ac0c SB |
395 | { |
396 | return cfpkt_priv(pkt)->erronous; | |
397 | } | |
15c9ac0c SB |
398 | |
399 | struct caif_payload_info *cfpkt_info(struct cfpkt *pkt) | |
400 | { | |
401 | return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb; | |
402 | } |