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