1 /* 2 * Copyright (C) ST-Ericsson AB 2010 3 * Author: Sjur Brendeland 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 208 /* All set to put the last SKB and optionally write data there. */ 209 to = pskb_put(skb, lastskb, len); 210 if (likely(data)) 211 memcpy(to, data, len); 212 return 0; 213 } 214 215 inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data) 216 { 217 return cfpkt_add_body(pkt, &data, 1); 218 } 219 220 int cfpkt_add_head(struct cfpkt *pkt, const void *data2, u16 len) 221 { 222 struct sk_buff *skb = pkt_to_skb(pkt); 223 struct sk_buff *lastskb; 224 u8 *to; 225 const u8 *data = data2; 226 int ret; 227 if (unlikely(is_erronous(pkt))) 228 return -EPROTO; 229 if (unlikely(skb_headroom(skb) < len)) { 230 PKT_ERROR(pkt, "no headroom\n"); 231 return -EPROTO; 232 } 233 234 /* Make sure data is writable */ 235 ret = skb_cow_data(skb, 0, &lastskb); 236 if (unlikely(ret < 0)) { 237 PKT_ERROR(pkt, "cow failed\n"); 238 return ret; 239 } 240 241 to = skb_push(skb, len); 242 memcpy(to, data, len); 243 return 0; 244 } 245 EXPORT_SYMBOL(cfpkt_add_head); 246 247 inline int cfpkt_add_trail(struct cfpkt *pkt, const void *data, u16 len) 248 { 249 return cfpkt_add_body(pkt, data, len); 250 } 251 252 inline u16 cfpkt_getlen(struct cfpkt *pkt) 253 { 254 struct sk_buff *skb = pkt_to_skb(pkt); 255 return skb->len; 256 } 257 258 inline u16 cfpkt_iterate(struct cfpkt *pkt, 259 u16 (*iter_func)(u16, void *, u16), 260 u16 data) 261 { 262 /* 263 * Don't care about the performance hit of linearizing, 264 * Checksum should not be used on high-speed interfaces anyway. 265 */ 266 if (unlikely(is_erronous(pkt))) 267 return -EPROTO; 268 if (unlikely(skb_linearize(&pkt->skb) != 0)) { 269 PKT_ERROR(pkt, "linearize failed\n"); 270 return -EPROTO; 271 } 272 return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt)); 273 } 274 275 int cfpkt_setlen(struct cfpkt *pkt, u16 len) 276 { 277 struct sk_buff *skb = pkt_to_skb(pkt); 278 279 280 if (unlikely(is_erronous(pkt))) 281 return -EPROTO; 282 283 if (likely(len <= skb->len)) { 284 if (unlikely(skb->data_len)) 285 ___pskb_trim(skb, len); 286 else 287 skb_trim(skb, len); 288 289 return cfpkt_getlen(pkt); 290 } 291 292 /* Need to expand SKB */ 293 if (unlikely(!cfpkt_pad_trail(pkt, len - skb->len))) 294 PKT_ERROR(pkt, "skb_pad_trail failed\n"); 295 296 return cfpkt_getlen(pkt); 297 } 298 299 struct cfpkt *cfpkt_append(struct cfpkt *dstpkt, 300 struct cfpkt *addpkt, 301 u16 expectlen) 302 { 303 struct sk_buff *dst = pkt_to_skb(dstpkt); 304 struct sk_buff *add = pkt_to_skb(addpkt); 305 u16 addlen = skb_headlen(add); 306 u16 neededtailspace; 307 struct sk_buff *tmp; 308 u16 dstlen; 309 u16 createlen; 310 if (unlikely(is_erronous(dstpkt) || is_erronous(addpkt))) { 311 return dstpkt; 312 } 313 if (expectlen > addlen) 314 neededtailspace = expectlen; 315 else 316 neededtailspace = addlen; 317 318 if (dst->tail + neededtailspace > dst->end) { 319 /* Create a dumplicate of 'dst' with more tail space */ 320 struct cfpkt *tmppkt; 321 dstlen = skb_headlen(dst); 322 createlen = dstlen + neededtailspace; 323 tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX); 324 if (tmppkt == NULL) 325 return NULL; 326 tmp = pkt_to_skb(tmppkt); 327 skb_set_tail_pointer(tmp, dstlen); 328 tmp->len = dstlen; 329 memcpy(tmp->data, dst->data, dstlen); 330 cfpkt_destroy(dstpkt); 331 dst = tmp; 332 } 333 memcpy(skb_tail_pointer(dst), add->data, skb_headlen(add)); 334 cfpkt_destroy(addpkt); 335 dst->tail += addlen; 336 dst->len += addlen; 337 return skb_to_pkt(dst); 338 } 339 340 struct cfpkt *cfpkt_split(struct cfpkt *pkt, u16 pos) 341 { 342 struct sk_buff *skb2; 343 struct sk_buff *skb = pkt_to_skb(pkt); 344 struct cfpkt *tmppkt; 345 u8 *split = skb->data + pos; 346 u16 len2nd = skb_tail_pointer(skb) - split; 347 348 if (unlikely(is_erronous(pkt))) 349 return NULL; 350 351 if (skb->data + pos > skb_tail_pointer(skb)) { 352 PKT_ERROR(pkt, "trying to split beyond end of packet\n"); 353 return NULL; 354 } 355 356 /* Create a new packet for the second part of the data */ 357 tmppkt = cfpkt_create_pfx(len2nd + PKT_PREFIX + PKT_POSTFIX, 358 PKT_PREFIX); 359 if (tmppkt == NULL) 360 return NULL; 361 skb2 = pkt_to_skb(tmppkt); 362 363 364 if (skb2 == NULL) 365 return NULL; 366 367 /* Reduce the length of the original packet */ 368 skb_set_tail_pointer(skb, pos); 369 skb->len = pos; 370 371 memcpy(skb2->data, split, len2nd); 372 skb2->tail += len2nd; 373 skb2->len += len2nd; 374 skb2->priority = skb->priority; 375 return skb_to_pkt(skb2); 376 } 377 378 bool cfpkt_erroneous(struct cfpkt *pkt) 379 { 380 return cfpkt_priv(pkt)->erronous; 381 } 382 383 struct caif_payload_info *cfpkt_info(struct cfpkt *pkt) 384 { 385 return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb; 386 } 387 EXPORT_SYMBOL(cfpkt_info); 388 389 void cfpkt_set_prio(struct cfpkt *pkt, int prio) 390 { 391 pkt_to_skb(pkt)->priority = prio; 392 } 393 EXPORT_SYMBOL(cfpkt_set_prio); 394