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