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 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 inline bool cfpkt_more(struct cfpkt *pkt) 108 { 109 struct sk_buff *skb = pkt_to_skb(pkt); 110 return skb->len > 0; 111 } 112 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 } 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)) { 132 PKT_ERROR(pkt, "read beyond end of packet\n"); 133 return -EPROTO; 134 } 135 136 if (unlikely(len > skb_headlen(skb))) { 137 if (unlikely(skb_linearize(skb) != 0)) { 138 PKT_ERROR(pkt, "linearize failed\n"); 139 return -EPROTO; 140 } 141 } 142 from = skb_pull(skb, len); 143 from -= len; 144 if (data) 145 memcpy(data, from, len); 146 return 0; 147 } 148 EXPORT_SYMBOL(cfpkt_extr_head); 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 int cfpkt_pad_trail(struct cfpkt *pkt, u16 len) 173 { 174 return cfpkt_add_body(pkt, NULL, len); 175 } 176 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)) { 203 PKT_ERROR(pkt, "cow failed\n"); 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) { 212 pr_warn("Packet is non-linear\n"); 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 } 224 225 inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data) 226 { 227 return cfpkt_add_body(pkt, &data, 1); 228 } 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; 236 int ret; 237 if (unlikely(is_erronous(pkt))) 238 return -EPROTO; 239 if (unlikely(skb_headroom(skb) < len)) { 240 PKT_ERROR(pkt, "no headroom\n"); 241 return -EPROTO; 242 } 243 244 /* Make sure data is writable */ 245 ret = skb_cow_data(skb, 0, &lastskb); 246 if (unlikely(ret < 0)) { 247 PKT_ERROR(pkt, "cow failed\n"); 248 return ret; 249 } 250 251 to = skb_push(skb, len); 252 memcpy(to, data, len); 253 return 0; 254 } 255 EXPORT_SYMBOL(cfpkt_add_head); 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 } 261 262 inline u16 cfpkt_getlen(struct cfpkt *pkt) 263 { 264 struct sk_buff *skb = pkt_to_skb(pkt); 265 return skb->len; 266 } 267 268 inline u16 cfpkt_iterate(struct cfpkt *pkt, 269 u16 (*iter_func)(u16, void *, u16), 270 u16 data) 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)) { 279 PKT_ERROR(pkt, "linearize failed\n"); 280 return -EPROTO; 281 } 282 return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt)); 283 } 284 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))) 304 PKT_ERROR(pkt, "skb_pad_trail failed\n"); 305 306 return cfpkt_getlen(pkt); 307 } 308 309 struct cfpkt *cfpkt_append(struct cfpkt *dstpkt, 310 struct cfpkt *addpkt, 311 u16 expectlen) 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))) { 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 */ 330 struct cfpkt *tmppkt; 331 dstlen = skb_headlen(dst); 332 createlen = dstlen + neededtailspace; 333 tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX); 334 if (tmppkt == NULL) 335 return NULL; 336 tmp = pkt_to_skb(tmppkt); 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 } 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); 354 struct cfpkt *tmppkt; 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)) { 362 PKT_ERROR(pkt, "trying to split beyond end of packet\n"); 363 return NULL; 364 } 365 366 /* Create a new packet for the second part of the data */ 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 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; 384 skb2->priority = skb->priority; 385 return skb_to_pkt(skb2); 386 } 387 388 bool cfpkt_erroneous(struct cfpkt *pkt) 389 { 390 return cfpkt_priv(pkt)->erronous; 391 } 392 393 struct caif_payload_info *cfpkt_info(struct cfpkt *pkt) 394 { 395 return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb; 396 } 397 EXPORT_SYMBOL(cfpkt_info); 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); 404