1 /* 2 * Copyright (C) ST-Ericsson AB 2010 3 * Author: Sjur Brendeland/sjur.brandeland@stericsson.com 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 <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 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 } 100 101 void cfpkt_destroy(struct cfpkt *pkt) 102 { 103 struct sk_buff *skb = pkt_to_skb(pkt); 104 kfree_skb(skb); 105 } 106 107 108 inline bool cfpkt_more(struct cfpkt *pkt) 109 { 110 struct sk_buff *skb = pkt_to_skb(pkt); 111 return skb->len > 0; 112 } 113 114 115 int cfpkt_peek_head(struct cfpkt *pkt, void *data, u16 len) 116 { 117 struct sk_buff *skb = pkt_to_skb(pkt); 118 if (skb_headlen(skb) >= len) { 119 memcpy(data, skb->data, len); 120 return 0; 121 } 122 return !cfpkt_extr_head(pkt, data, len) && 123 !cfpkt_add_head(pkt, data, len); 124 } 125 126 int cfpkt_extr_head(struct cfpkt *pkt, void *data, u16 len) 127 { 128 struct sk_buff *skb = pkt_to_skb(pkt); 129 u8 *from; 130 if (unlikely(is_erronous(pkt))) 131 return -EPROTO; 132 133 if (unlikely(len > skb->len)) { 134 PKT_ERROR(pkt, "read beyond end of packet\n"); 135 return -EPROTO; 136 } 137 138 if (unlikely(len > skb_headlen(skb))) { 139 if (unlikely(skb_linearize(skb) != 0)) { 140 PKT_ERROR(pkt, "linearize failed\n"); 141 return -EPROTO; 142 } 143 } 144 from = skb_pull(skb, len); 145 from -= len; 146 memcpy(data, from, len); 147 return 0; 148 } 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)) { 159 PKT_ERROR(pkt, "linearize failed\n"); 160 return -EPROTO; 161 } 162 if (unlikely(skb->data + len > skb_tail_pointer(skb))) { 163 PKT_ERROR(pkt, "read beyond end of packet\n"); 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 } 171 172 173 int cfpkt_pad_trail(struct cfpkt *pkt, u16 len) 174 { 175 return cfpkt_add_body(pkt, NULL, len); 176 } 177 178 179 int cfpkt_add_body(struct cfpkt *pkt, const void *data, u16 len) 180 { 181 struct sk_buff *skb = pkt_to_skb(pkt); 182 struct sk_buff *lastskb; 183 u8 *to; 184 u16 addlen = 0; 185 186 187 if (unlikely(is_erronous(pkt))) 188 return -EPROTO; 189 190 lastskb = skb; 191 192 /* Check whether we need to add space at the tail */ 193 if (unlikely(skb_tailroom(skb) < len)) { 194 if (likely(len < PKT_LEN_WHEN_EXTENDING)) 195 addlen = PKT_LEN_WHEN_EXTENDING; 196 else 197 addlen = len; 198 } 199 200 /* Check whether we need to change the SKB before writing to the tail */ 201 if (unlikely((addlen > 0) || skb_cloned(skb) || skb_shared(skb))) { 202 203 /* Make sure data is writable */ 204 if (unlikely(skb_cow_data(skb, addlen, &lastskb) < 0)) { 205 PKT_ERROR(pkt, "cow failed\n"); 206 return -EPROTO; 207 } 208 /* 209 * Is the SKB non-linear after skb_cow_data()? If so, we are 210 * going to add data to the last SKB, so we need to adjust 211 * lengths of the top SKB. 212 */ 213 if (lastskb != skb) { 214 pr_warn("Packet is non-linear\n"); 215 skb->len += len; 216 skb->data_len += len; 217 } 218 } 219 220 /* All set to put the last SKB and optionally write data there. */ 221 to = skb_put(lastskb, len); 222 if (likely(data)) 223 memcpy(to, data, len); 224 return 0; 225 } 226 227 inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data) 228 { 229 return cfpkt_add_body(pkt, &data, 1); 230 } 231 232 int cfpkt_add_head(struct cfpkt *pkt, const void *data2, u16 len) 233 { 234 struct sk_buff *skb = pkt_to_skb(pkt); 235 struct sk_buff *lastskb; 236 u8 *to; 237 const u8 *data = data2; 238 int ret; 239 if (unlikely(is_erronous(pkt))) 240 return -EPROTO; 241 if (unlikely(skb_headroom(skb) < len)) { 242 PKT_ERROR(pkt, "no headroom\n"); 243 return -EPROTO; 244 } 245 246 /* Make sure data is writable */ 247 ret = skb_cow_data(skb, 0, &lastskb); 248 if (unlikely(ret < 0)) { 249 PKT_ERROR(pkt, "cow failed\n"); 250 return ret; 251 } 252 253 to = skb_push(skb, len); 254 memcpy(to, data, len); 255 return 0; 256 } 257 258 259 inline int cfpkt_add_trail(struct cfpkt *pkt, const void *data, u16 len) 260 { 261 return cfpkt_add_body(pkt, data, len); 262 } 263 264 265 inline u16 cfpkt_getlen(struct cfpkt *pkt) 266 { 267 struct sk_buff *skb = pkt_to_skb(pkt); 268 return skb->len; 269 } 270 271 272 inline u16 cfpkt_iterate(struct cfpkt *pkt, 273 u16 (*iter_func)(u16, void *, u16), 274 u16 data) 275 { 276 /* 277 * Don't care about the performance hit of linearizing, 278 * Checksum should not be used on high-speed interfaces anyway. 279 */ 280 if (unlikely(is_erronous(pkt))) 281 return -EPROTO; 282 if (unlikely(skb_linearize(&pkt->skb) != 0)) { 283 PKT_ERROR(pkt, "linearize failed\n"); 284 return -EPROTO; 285 } 286 return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt)); 287 } 288 289 290 int cfpkt_setlen(struct cfpkt *pkt, u16 len) 291 { 292 struct sk_buff *skb = pkt_to_skb(pkt); 293 294 295 if (unlikely(is_erronous(pkt))) 296 return -EPROTO; 297 298 if (likely(len <= skb->len)) { 299 if (unlikely(skb->data_len)) 300 ___pskb_trim(skb, len); 301 else 302 skb_trim(skb, len); 303 304 return cfpkt_getlen(pkt); 305 } 306 307 /* Need to expand SKB */ 308 if (unlikely(!cfpkt_pad_trail(pkt, len - skb->len))) 309 PKT_ERROR(pkt, "skb_pad_trail failed\n"); 310 311 return cfpkt_getlen(pkt); 312 } 313 314 struct cfpkt *cfpkt_append(struct cfpkt *dstpkt, 315 struct cfpkt *addpkt, 316 u16 expectlen) 317 { 318 struct sk_buff *dst = pkt_to_skb(dstpkt); 319 struct sk_buff *add = pkt_to_skb(addpkt); 320 u16 addlen = skb_headlen(add); 321 u16 neededtailspace; 322 struct sk_buff *tmp; 323 u16 dstlen; 324 u16 createlen; 325 if (unlikely(is_erronous(dstpkt) || is_erronous(addpkt))) { 326 return dstpkt; 327 } 328 if (expectlen > addlen) 329 neededtailspace = expectlen; 330 else 331 neededtailspace = addlen; 332 333 if (dst->tail + neededtailspace > dst->end) { 334 /* Create a dumplicate of 'dst' with more tail space */ 335 struct cfpkt *tmppkt; 336 dstlen = skb_headlen(dst); 337 createlen = dstlen + neededtailspace; 338 tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX); 339 if (tmppkt == NULL) 340 return NULL; 341 tmp = pkt_to_skb(tmppkt); 342 skb_set_tail_pointer(tmp, dstlen); 343 tmp->len = dstlen; 344 memcpy(tmp->data, dst->data, dstlen); 345 cfpkt_destroy(dstpkt); 346 dst = tmp; 347 } 348 memcpy(skb_tail_pointer(dst), add->data, skb_headlen(add)); 349 cfpkt_destroy(addpkt); 350 dst->tail += addlen; 351 dst->len += addlen; 352 return skb_to_pkt(dst); 353 } 354 355 struct cfpkt *cfpkt_split(struct cfpkt *pkt, u16 pos) 356 { 357 struct sk_buff *skb2; 358 struct sk_buff *skb = pkt_to_skb(pkt); 359 struct cfpkt *tmppkt; 360 u8 *split = skb->data + pos; 361 u16 len2nd = skb_tail_pointer(skb) - split; 362 363 if (unlikely(is_erronous(pkt))) 364 return NULL; 365 366 if (skb->data + pos > skb_tail_pointer(skb)) { 367 PKT_ERROR(pkt, "trying to split beyond end of packet\n"); 368 return NULL; 369 } 370 371 /* Create a new packet for the second part of the data */ 372 tmppkt = cfpkt_create_pfx(len2nd + PKT_PREFIX + PKT_POSTFIX, 373 PKT_PREFIX); 374 if (tmppkt == NULL) 375 return NULL; 376 skb2 = pkt_to_skb(tmppkt); 377 378 379 if (skb2 == NULL) 380 return NULL; 381 382 /* Reduce the length of the original packet */ 383 skb_set_tail_pointer(skb, pos); 384 skb->len = pos; 385 386 memcpy(skb2->data, split, len2nd); 387 skb2->tail += len2nd; 388 skb2->len += len2nd; 389 return skb_to_pkt(skb2); 390 } 391 392 bool cfpkt_erroneous(struct cfpkt *pkt) 393 { 394 return cfpkt_priv(pkt)->erronous; 395 } 396 397 struct caif_payload_info *cfpkt_info(struct cfpkt *pkt) 398 { 399 return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb; 400 } 401