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