Blame view
net/caif/cfpkt_skbuff.c
8.5 KB
15c9ac0c8 net-caif: add CAI... |
1 2 3 4 5 |
/* * Copyright (C) ST-Ericsson AB 2010 * Author: Sjur Brendeland/sjur.brandeland@stericsson.com * 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 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 |
{ return (struct cfpkt *) skb; } 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; if (likely(in_interrupt())) skb = alloc_skb(len + pfx, GFP_ATOMIC); else skb = alloc_skb(len + pfx, GFP_KERNEL); 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... |
94 95 96 97 98 99 |
void cfpkt_destroy(struct cfpkt *pkt) { struct sk_buff *skb = pkt_to_skb(pkt); kfree_skb(skb); } |
3f874adc4 caif: remove unes... |
100 |
|
15c9ac0c8 net-caif: add CAI... |
101 102 103 104 105 106 |
inline bool cfpkt_more(struct cfpkt *pkt) { struct sk_buff *skb = pkt_to_skb(pkt); return skb->len > 0; } |
3f874adc4 caif: remove unes... |
107 |
|
15c9ac0c8 net-caif: add CAI... |
108 109 110 111 112 113 114 115 116 117 118 |
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... |
119 120 121 122 123 124 125 126 127 |
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 |
128 129 |
PKT_ERROR(pkt, "read beyond end of packet "); |
15c9ac0c8 net-caif: add CAI... |
130 131 132 133 134 |
return -EPROTO; } if (unlikely(len > skb_headlen(skb))) { if (unlikely(skb_linearize(skb) != 0)) { |
b31fa5bad net/caif: Use pr_fmt |
135 136 |
PKT_ERROR(pkt, "linearize failed "); |
15c9ac0c8 net-caif: add CAI... |
137 138 139 140 141 142 143 144 |
return -EPROTO; } } from = skb_pull(skb, len); from -= len; memcpy(data, from, len); return 0; } |
15c9ac0c8 net-caif: add CAI... |
145 146 147 148 149 150 151 152 153 154 |
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 |
155 156 |
PKT_ERROR(pkt, "linearize failed "); |
15c9ac0c8 net-caif: add CAI... |
157 158 159 |
return -EPROTO; } if (unlikely(skb->data + len > skb_tail_pointer(skb))) { |
b31fa5bad net/caif: Use pr_fmt |
160 161 |
PKT_ERROR(pkt, "read beyond end of packet "); |
15c9ac0c8 net-caif: add CAI... |
162 163 164 165 166 167 168 |
return -EPROTO; } from = skb_tail_pointer(skb) - len; skb_trim(skb, skb->len - len); memcpy(data, from, len); return 0; } |
3f874adc4 caif: remove unes... |
169 |
|
15c9ac0c8 net-caif: add CAI... |
170 171 172 173 174 |
int cfpkt_pad_trail(struct cfpkt *pkt, u16 len) { return cfpkt_add_body(pkt, NULL, len); } |
3f874adc4 caif: remove unes... |
175 |
|
15c9ac0c8 net-caif: add CAI... |
176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 |
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 |
203 204 |
PKT_ERROR(pkt, "cow failed "); |
15c9ac0c8 net-caif: add CAI... |
205 206 207 208 209 210 211 212 |
return -EPROTO; } /* * Is the SKB non-linear after skb_cow_data()? If so, we are * going to add data to the last SKB, so we need to adjust * lengths of the top SKB. */ if (lastskb != skb) { |
b31fa5bad net/caif: Use pr_fmt |
213 214 |
pr_warn("Packet is non-linear "); |
15c9ac0c8 net-caif: add CAI... |
215 216 217 218 219 220 221 222 223 224 225 |
skb->len += len; skb->data_len += len; } } /* All set to put the last SKB and optionally write data there. */ to = skb_put(lastskb, len); if (likely(data)) memcpy(to, data, len); return 0; } |
15c9ac0c8 net-caif: add CAI... |
226 227 228 229 230 |
inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data) { return cfpkt_add_body(pkt, &data, 1); } |
15c9ac0c8 net-caif: add CAI... |
231 232 233 234 235 236 237 |
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... |
238 |
int ret; |
15c9ac0c8 net-caif: add CAI... |
239 240 241 |
if (unlikely(is_erronous(pkt))) return -EPROTO; if (unlikely(skb_headroom(skb) < len)) { |
b31fa5bad net/caif: Use pr_fmt |
242 243 |
PKT_ERROR(pkt, "no headroom "); |
15c9ac0c8 net-caif: add CAI... |
244 245 246 247 |
return -EPROTO; } /* Make sure data is writable */ |
638e628a6 caif: Bugfix - ha... |
248 249 |
ret = skb_cow_data(skb, 0, &lastskb); if (unlikely(ret < 0)) { |
b31fa5bad net/caif: Use pr_fmt |
250 251 |
PKT_ERROR(pkt, "cow failed "); |
638e628a6 caif: Bugfix - ha... |
252 |
return ret; |
15c9ac0c8 net-caif: add CAI... |
253 254 255 256 257 258 |
} to = skb_push(skb, len); memcpy(to, data, len); return 0; } |
3f874adc4 caif: remove unes... |
259 |
|
15c9ac0c8 net-caif: add CAI... |
260 261 262 263 264 |
inline int cfpkt_add_trail(struct cfpkt *pkt, const void *data, u16 len) { return cfpkt_add_body(pkt, data, len); } |
3f874adc4 caif: remove unes... |
265 |
|
15c9ac0c8 net-caif: add CAI... |
266 267 268 269 270 271 |
inline u16 cfpkt_getlen(struct cfpkt *pkt) { struct sk_buff *skb = pkt_to_skb(pkt); return skb->len; } |
3f874adc4 caif: remove unes... |
272 |
|
15c9ac0c8 net-caif: add CAI... |
273 274 275 276 277 278 279 280 281 282 283 284 |
inline u16 cfpkt_iterate(struct cfpkt *pkt, u16 (*iter_func)(u16, void *, u16), u16 data) { /* * 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 |
285 286 |
PKT_ERROR(pkt, "linearize failed "); |
15c9ac0c8 net-caif: add CAI... |
287 288 289 290 |
return -EPROTO; } return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt)); } |
3f874adc4 caif: remove unes... |
291 |
|
15c9ac0c8 net-caif: add CAI... |
292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 |
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); return cfpkt_getlen(pkt); } /* Need to expand SKB */ if (unlikely(!cfpkt_pad_trail(pkt, len - skb->len))) |
b31fa5bad net/caif: Use pr_fmt |
312 313 |
PKT_ERROR(pkt, "skb_pad_trail failed "); |
15c9ac0c8 net-caif: add CAI... |
314 315 316 |
return cfpkt_getlen(pkt); } |
15c9ac0c8 net-caif: add CAI... |
317 |
|
15c9ac0c8 net-caif: add CAI... |
318 319 320 321 322 323 324 325 326 327 328 329 |
struct cfpkt *cfpkt_append(struct cfpkt *dstpkt, struct cfpkt *addpkt, u16 expectlen) { 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... |
330 331 332 333 334 335 336 337 338 |
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... |
339 |
struct cfpkt *tmppkt; |
15c9ac0c8 net-caif: add CAI... |
340 341 |
dstlen = skb_headlen(dst); createlen = dstlen + neededtailspace; |
638e628a6 caif: Bugfix - ha... |
342 343 |
tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX); if (tmppkt == NULL) |
15c9ac0c8 net-caif: add CAI... |
344 |
return NULL; |
638e628a6 caif: Bugfix - ha... |
345 |
tmp = pkt_to_skb(tmppkt); |
15c9ac0c8 net-caif: add CAI... |
346 347 348 349 350 351 352 353 354 355 356 357 |
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... |
358 359 360 361 362 |
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... |
363 |
struct cfpkt *tmppkt; |
15c9ac0c8 net-caif: add CAI... |
364 365 366 367 368 369 370 |
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 |
371 372 |
PKT_ERROR(pkt, "trying to split beyond end of packet "); |
15c9ac0c8 net-caif: add CAI... |
373 374 375 376 |
return NULL; } /* Create a new packet for the second part of the data */ |
638e628a6 caif: Bugfix - ha... |
377 378 379 380 381 |
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... |
382 383 384 385 386 387 388 389 390 391 392 393 394 |
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; return skb_to_pkt(skb2); } |
15c9ac0c8 net-caif: add CAI... |
395 |
|
73d6ac633 caif: code cleanup |
396 |
bool cfpkt_erroneous(struct cfpkt *pkt) |
15c9ac0c8 net-caif: add CAI... |
397 398 399 |
{ return cfpkt_priv(pkt)->erronous; } |
15c9ac0c8 net-caif: add CAI... |
400 401 402 403 404 |
struct caif_payload_info *cfpkt_info(struct cfpkt *pkt) { return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb; } |