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