1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * Copyright (C) ST-Ericsson AB 2010 |
4 | * Author: Sjur Brendeland |
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/export.h> |
12 | #include <net/caif/cfpkt.h> |
13 | |
14 | #define PKT_PREFIX 48 |
15 | #define PKT_POSTFIX 2 |
16 | #define PKT_LEN_WHEN_EXTENDING 128 |
17 | #define PKT_ERROR(pkt, errmsg) \ |
18 | do { \ |
19 | cfpkt_priv(pkt)->erronous = true; \ |
20 | skb_reset_tail_pointer(&pkt->skb); \ |
21 | pr_warn(errmsg); \ |
22 | } while (0) |
23 | |
24 | struct 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 | */ |
35 | struct cfpkt { |
36 | struct sk_buff skb; |
37 | }; |
38 | |
39 | /* Private data inside SKB */ |
40 | struct cfpkt_priv_data { |
41 | struct dev_info dev_info; |
42 | bool erronous; |
43 | }; |
44 | |
45 | static inline struct cfpkt_priv_data *cfpkt_priv(struct cfpkt *pkt) |
46 | { |
47 | return (struct cfpkt_priv_data *) pkt->skb.cb; |
48 | } |
49 | |
50 | static inline bool is_erronous(struct cfpkt *pkt) |
51 | { |
52 | return cfpkt_priv(pkt)->erronous; |
53 | } |
54 | |
55 | static inline struct sk_buff *pkt_to_skb(struct cfpkt *pkt) |
56 | { |
57 | return &pkt->skb; |
58 | } |
59 | |
60 | static inline struct cfpkt *skb_to_pkt(struct sk_buff *skb) |
61 | { |
62 | return (struct cfpkt *) skb; |
63 | } |
64 | |
65 | struct cfpkt *cfpkt_fromnative(enum caif_direction dir, void *nativepkt) |
66 | { |
67 | struct cfpkt *pkt = skb_to_pkt(skb: nativepkt); |
68 | cfpkt_priv(pkt)->erronous = false; |
69 | return pkt; |
70 | } |
71 | EXPORT_SYMBOL(cfpkt_fromnative); |
72 | |
73 | void *cfpkt_tonative(struct cfpkt *pkt) |
74 | { |
75 | return (void *) pkt; |
76 | } |
77 | EXPORT_SYMBOL(cfpkt_tonative); |
78 | |
79 | static struct cfpkt *cfpkt_create_pfx(u16 len, u16 pfx) |
80 | { |
81 | struct sk_buff *skb; |
82 | |
83 | skb = alloc_skb(size: len + pfx, GFP_ATOMIC); |
84 | if (unlikely(skb == NULL)) |
85 | return NULL; |
86 | |
87 | skb_reserve(skb, len: pfx); |
88 | return skb_to_pkt(skb); |
89 | } |
90 | |
91 | inline struct cfpkt *cfpkt_create(u16 len) |
92 | { |
93 | return cfpkt_create_pfx(len: len + PKT_POSTFIX, PKT_PREFIX); |
94 | } |
95 | |
96 | void cfpkt_destroy(struct cfpkt *pkt) |
97 | { |
98 | struct sk_buff *skb = pkt_to_skb(pkt); |
99 | kfree_skb(skb); |
100 | } |
101 | |
102 | inline bool cfpkt_more(struct cfpkt *pkt) |
103 | { |
104 | struct sk_buff *skb = pkt_to_skb(pkt); |
105 | return skb->len > 0; |
106 | } |
107 | |
108 | int cfpkt_peek_head(struct cfpkt *pkt, void *data, u16 len) |
109 | { |
110 | struct sk_buff *skb = pkt_to_skb(pkt); |
111 | if (skb_headlen(skb) >= len) { |
112 | memcpy(data, skb->data, len); |
113 | return 0; |
114 | } |
115 | return !cfpkt_extr_head(pkt, data, len) && |
116 | !cfpkt_add_head(pkt, data, len); |
117 | } |
118 | |
119 | int cfpkt_extr_head(struct cfpkt *pkt, void *data, u16 len) |
120 | { |
121 | struct sk_buff *skb = pkt_to_skb(pkt); |
122 | u8 *from; |
123 | if (unlikely(is_erronous(pkt))) |
124 | return -EPROTO; |
125 | |
126 | if (unlikely(len > skb->len)) { |
127 | PKT_ERROR(pkt, "read beyond end of packet\n" ); |
128 | return -EPROTO; |
129 | } |
130 | |
131 | if (unlikely(len > skb_headlen(skb))) { |
132 | if (unlikely(skb_linearize(skb) != 0)) { |
133 | PKT_ERROR(pkt, "linearize failed\n" ); |
134 | return -EPROTO; |
135 | } |
136 | } |
137 | from = skb_pull(skb, len); |
138 | from -= len; |
139 | if (data) |
140 | memcpy(data, from, len); |
141 | return 0; |
142 | } |
143 | EXPORT_SYMBOL(cfpkt_extr_head); |
144 | |
145 | int cfpkt_extr_trail(struct cfpkt *pkt, void *dta, u16 len) |
146 | { |
147 | struct sk_buff *skb = pkt_to_skb(pkt); |
148 | u8 *data = dta; |
149 | u8 *from; |
150 | if (unlikely(is_erronous(pkt))) |
151 | return -EPROTO; |
152 | |
153 | if (unlikely(skb_linearize(skb) != 0)) { |
154 | PKT_ERROR(pkt, "linearize failed\n" ); |
155 | return -EPROTO; |
156 | } |
157 | if (unlikely(skb->data + len > skb_tail_pointer(skb))) { |
158 | PKT_ERROR(pkt, "read beyond end of packet\n" ); |
159 | return -EPROTO; |
160 | } |
161 | from = skb_tail_pointer(skb) - len; |
162 | skb_trim(skb, len: skb->len - len); |
163 | memcpy(data, from, len); |
164 | return 0; |
165 | } |
166 | |
167 | int cfpkt_pad_trail(struct cfpkt *pkt, u16 len) |
168 | { |
169 | return cfpkt_add_body(pkt, NULL, len); |
170 | } |
171 | |
172 | int cfpkt_add_body(struct cfpkt *pkt, const void *data, u16 len) |
173 | { |
174 | struct sk_buff *skb = pkt_to_skb(pkt); |
175 | struct sk_buff *lastskb; |
176 | u8 *to; |
177 | u16 addlen = 0; |
178 | |
179 | |
180 | if (unlikely(is_erronous(pkt))) |
181 | return -EPROTO; |
182 | |
183 | lastskb = skb; |
184 | |
185 | /* Check whether we need to add space at the tail */ |
186 | if (unlikely(skb_tailroom(skb) < len)) { |
187 | if (likely(len < PKT_LEN_WHEN_EXTENDING)) |
188 | addlen = PKT_LEN_WHEN_EXTENDING; |
189 | else |
190 | addlen = len; |
191 | } |
192 | |
193 | /* Check whether we need to change the SKB before writing to the tail */ |
194 | if (unlikely((addlen > 0) || skb_cloned(skb) || skb_shared(skb))) { |
195 | |
196 | /* Make sure data is writable */ |
197 | if (unlikely(skb_cow_data(skb, addlen, &lastskb) < 0)) { |
198 | PKT_ERROR(pkt, "cow failed\n" ); |
199 | return -EPROTO; |
200 | } |
201 | } |
202 | |
203 | /* All set to put the last SKB and optionally write data there. */ |
204 | to = pskb_put(skb, tail: lastskb, len); |
205 | if (likely(data)) |
206 | memcpy(to, data, len); |
207 | return 0; |
208 | } |
209 | |
210 | inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data) |
211 | { |
212 | return cfpkt_add_body(pkt, data: &data, len: 1); |
213 | } |
214 | |
215 | int cfpkt_add_head(struct cfpkt *pkt, const void *data2, u16 len) |
216 | { |
217 | struct sk_buff *skb = pkt_to_skb(pkt); |
218 | struct sk_buff *lastskb; |
219 | u8 *to; |
220 | const u8 *data = data2; |
221 | int ret; |
222 | if (unlikely(is_erronous(pkt))) |
223 | return -EPROTO; |
224 | if (unlikely(skb_headroom(skb) < len)) { |
225 | PKT_ERROR(pkt, "no headroom\n" ); |
226 | return -EPROTO; |
227 | } |
228 | |
229 | /* Make sure data is writable */ |
230 | ret = skb_cow_data(skb, tailbits: 0, trailer: &lastskb); |
231 | if (unlikely(ret < 0)) { |
232 | PKT_ERROR(pkt, "cow failed\n" ); |
233 | return ret; |
234 | } |
235 | |
236 | to = skb_push(skb, len); |
237 | memcpy(to, data, len); |
238 | return 0; |
239 | } |
240 | EXPORT_SYMBOL(cfpkt_add_head); |
241 | |
242 | inline int cfpkt_add_trail(struct cfpkt *pkt, const void *data, u16 len) |
243 | { |
244 | return cfpkt_add_body(pkt, data, len); |
245 | } |
246 | |
247 | inline u16 cfpkt_getlen(struct cfpkt *pkt) |
248 | { |
249 | struct sk_buff *skb = pkt_to_skb(pkt); |
250 | return skb->len; |
251 | } |
252 | |
253 | int cfpkt_iterate(struct cfpkt *pkt, |
254 | u16 (*iter_func)(u16, void *, u16), |
255 | u16 data) |
256 | { |
257 | /* |
258 | * Don't care about the performance hit of linearizing, |
259 | * Checksum should not be used on high-speed interfaces anyway. |
260 | */ |
261 | if (unlikely(is_erronous(pkt))) |
262 | return -EPROTO; |
263 | if (unlikely(skb_linearize(&pkt->skb) != 0)) { |
264 | PKT_ERROR(pkt, "linearize failed\n" ); |
265 | return -EPROTO; |
266 | } |
267 | return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt)); |
268 | } |
269 | |
270 | int cfpkt_setlen(struct cfpkt *pkt, u16 len) |
271 | { |
272 | struct sk_buff *skb = pkt_to_skb(pkt); |
273 | |
274 | |
275 | if (unlikely(is_erronous(pkt))) |
276 | return -EPROTO; |
277 | |
278 | if (likely(len <= skb->len)) { |
279 | if (unlikely(skb->data_len)) |
280 | ___pskb_trim(skb, len); |
281 | else |
282 | skb_trim(skb, len); |
283 | |
284 | return cfpkt_getlen(pkt); |
285 | } |
286 | |
287 | /* Need to expand SKB */ |
288 | if (unlikely(!cfpkt_pad_trail(pkt, len - skb->len))) |
289 | PKT_ERROR(pkt, "skb_pad_trail failed\n" ); |
290 | |
291 | return cfpkt_getlen(pkt); |
292 | } |
293 | |
294 | struct cfpkt *cfpkt_append(struct cfpkt *dstpkt, |
295 | struct cfpkt *addpkt, |
296 | u16 expectlen) |
297 | { |
298 | struct sk_buff *dst = pkt_to_skb(pkt: dstpkt); |
299 | struct sk_buff *add = pkt_to_skb(pkt: addpkt); |
300 | u16 addlen = skb_headlen(skb: add); |
301 | u16 neededtailspace; |
302 | struct sk_buff *tmp; |
303 | u16 dstlen; |
304 | u16 createlen; |
305 | if (unlikely(is_erronous(dstpkt) || is_erronous(addpkt))) { |
306 | return dstpkt; |
307 | } |
308 | if (expectlen > addlen) |
309 | neededtailspace = expectlen; |
310 | else |
311 | neededtailspace = addlen; |
312 | |
313 | if (dst->tail + neededtailspace > dst->end) { |
314 | /* Create a dumplicate of 'dst' with more tail space */ |
315 | struct cfpkt *tmppkt; |
316 | dstlen = skb_headlen(skb: dst); |
317 | createlen = dstlen + neededtailspace; |
318 | tmppkt = cfpkt_create(len: createlen + PKT_PREFIX + PKT_POSTFIX); |
319 | if (tmppkt == NULL) |
320 | return NULL; |
321 | tmp = pkt_to_skb(pkt: tmppkt); |
322 | skb_put_data(skb: tmp, data: dst->data, len: dstlen); |
323 | cfpkt_destroy(pkt: dstpkt); |
324 | dst = tmp; |
325 | } |
326 | skb_put_data(skb: dst, data: add->data, len: skb_headlen(skb: add)); |
327 | cfpkt_destroy(pkt: addpkt); |
328 | return skb_to_pkt(skb: dst); |
329 | } |
330 | |
331 | struct cfpkt *cfpkt_split(struct cfpkt *pkt, u16 pos) |
332 | { |
333 | struct sk_buff *skb2; |
334 | struct sk_buff *skb = pkt_to_skb(pkt); |
335 | struct cfpkt *tmppkt; |
336 | u8 *split = skb->data + pos; |
337 | u16 len2nd = skb_tail_pointer(skb) - split; |
338 | |
339 | if (unlikely(is_erronous(pkt))) |
340 | return NULL; |
341 | |
342 | if (skb->data + pos > skb_tail_pointer(skb)) { |
343 | PKT_ERROR(pkt, "trying to split beyond end of packet\n" ); |
344 | return NULL; |
345 | } |
346 | |
347 | /* Create a new packet for the second part of the data */ |
348 | tmppkt = cfpkt_create_pfx(len: len2nd + PKT_PREFIX + PKT_POSTFIX, |
349 | PKT_PREFIX); |
350 | if (tmppkt == NULL) |
351 | return NULL; |
352 | skb2 = pkt_to_skb(pkt: tmppkt); |
353 | |
354 | |
355 | if (skb2 == NULL) |
356 | return NULL; |
357 | |
358 | skb_put_data(skb: skb2, data: split, len: len2nd); |
359 | |
360 | /* Reduce the length of the original packet */ |
361 | skb_trim(skb, len: pos); |
362 | |
363 | skb2->priority = skb->priority; |
364 | return skb_to_pkt(skb: skb2); |
365 | } |
366 | |
367 | bool cfpkt_erroneous(struct cfpkt *pkt) |
368 | { |
369 | return cfpkt_priv(pkt)->erronous; |
370 | } |
371 | |
372 | struct caif_payload_info *cfpkt_info(struct cfpkt *pkt) |
373 | { |
374 | return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb; |
375 | } |
376 | EXPORT_SYMBOL(cfpkt_info); |
377 | |
378 | void cfpkt_set_prio(struct cfpkt *pkt, int prio) |
379 | { |
380 | pkt_to_skb(pkt)->priority = prio; |
381 | } |
382 | EXPORT_SYMBOL(cfpkt_set_prio); |
383 | |