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