115c9ac0cSSjur Braendeland /* 215c9ac0cSSjur Braendeland * Copyright (C) ST-Ericsson AB 2010 315c9ac0cSSjur Braendeland * Author: Sjur Brendeland/sjur.brandeland@stericsson.com 415c9ac0cSSjur Braendeland * License terms: GNU General Public License (GPL) version 2 515c9ac0cSSjur Braendeland */ 615c9ac0cSSjur Braendeland 715c9ac0cSSjur Braendeland #include <linux/string.h> 815c9ac0cSSjur Braendeland #include <linux/skbuff.h> 915c9ac0cSSjur Braendeland #include <linux/hardirq.h> 1015c9ac0cSSjur Braendeland #include <net/caif/cfpkt.h> 1115c9ac0cSSjur Braendeland 122aa40aefSSjur Braendeland #define PKT_PREFIX 16 132aa40aefSSjur Braendeland #define PKT_POSTFIX 2 1415c9ac0cSSjur Braendeland #define PKT_LEN_WHEN_EXTENDING 128 1515c9ac0cSSjur Braendeland #define PKT_ERROR(pkt, errmsg) do { \ 1615c9ac0cSSjur Braendeland cfpkt_priv(pkt)->erronous = true; \ 1715c9ac0cSSjur Braendeland skb_reset_tail_pointer(&pkt->skb); \ 1815c9ac0cSSjur Braendeland pr_warning("CAIF: " errmsg);\ 1915c9ac0cSSjur Braendeland } while (0) 2015c9ac0cSSjur Braendeland 2115c9ac0cSSjur Braendeland struct cfpktq { 2215c9ac0cSSjur Braendeland struct sk_buff_head head; 2315c9ac0cSSjur Braendeland atomic_t count; 2415c9ac0cSSjur Braendeland /* Lock protects count updates */ 2515c9ac0cSSjur Braendeland spinlock_t lock; 2615c9ac0cSSjur Braendeland }; 2715c9ac0cSSjur Braendeland 2815c9ac0cSSjur Braendeland /* 2915c9ac0cSSjur Braendeland * net/caif/ is generic and does not 3015c9ac0cSSjur Braendeland * understand SKB, so we do this typecast 3115c9ac0cSSjur Braendeland */ 3215c9ac0cSSjur Braendeland struct cfpkt { 3315c9ac0cSSjur Braendeland struct sk_buff skb; 3415c9ac0cSSjur Braendeland }; 3515c9ac0cSSjur Braendeland 3615c9ac0cSSjur Braendeland /* Private data inside SKB */ 3715c9ac0cSSjur Braendeland struct cfpkt_priv_data { 3815c9ac0cSSjur Braendeland struct dev_info dev_info; 3915c9ac0cSSjur Braendeland bool erronous; 4015c9ac0cSSjur Braendeland }; 4115c9ac0cSSjur Braendeland 4215c9ac0cSSjur Braendeland inline struct cfpkt_priv_data *cfpkt_priv(struct cfpkt *pkt) 4315c9ac0cSSjur Braendeland { 4415c9ac0cSSjur Braendeland return (struct cfpkt_priv_data *) pkt->skb.cb; 4515c9ac0cSSjur Braendeland } 4615c9ac0cSSjur Braendeland 4715c9ac0cSSjur Braendeland inline bool is_erronous(struct cfpkt *pkt) 4815c9ac0cSSjur Braendeland { 4915c9ac0cSSjur Braendeland return cfpkt_priv(pkt)->erronous; 5015c9ac0cSSjur Braendeland } 5115c9ac0cSSjur Braendeland 5215c9ac0cSSjur Braendeland inline struct sk_buff *pkt_to_skb(struct cfpkt *pkt) 5315c9ac0cSSjur Braendeland { 5415c9ac0cSSjur Braendeland return &pkt->skb; 5515c9ac0cSSjur Braendeland } 5615c9ac0cSSjur Braendeland 5715c9ac0cSSjur Braendeland inline struct cfpkt *skb_to_pkt(struct sk_buff *skb) 5815c9ac0cSSjur Braendeland { 5915c9ac0cSSjur Braendeland return (struct cfpkt *) skb; 6015c9ac0cSSjur Braendeland } 6115c9ac0cSSjur Braendeland 6215c9ac0cSSjur Braendeland 6315c9ac0cSSjur Braendeland struct cfpkt *cfpkt_fromnative(enum caif_direction dir, void *nativepkt) 6415c9ac0cSSjur Braendeland { 6515c9ac0cSSjur Braendeland struct cfpkt *pkt = skb_to_pkt(nativepkt); 6615c9ac0cSSjur Braendeland cfpkt_priv(pkt)->erronous = false; 6715c9ac0cSSjur Braendeland return pkt; 6815c9ac0cSSjur Braendeland } 6915c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_fromnative); 7015c9ac0cSSjur Braendeland 7115c9ac0cSSjur Braendeland void *cfpkt_tonative(struct cfpkt *pkt) 7215c9ac0cSSjur Braendeland { 7315c9ac0cSSjur Braendeland return (void *) pkt; 7415c9ac0cSSjur Braendeland } 7515c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_tonative); 7615c9ac0cSSjur Braendeland 7715c9ac0cSSjur Braendeland static struct cfpkt *cfpkt_create_pfx(u16 len, u16 pfx) 7815c9ac0cSSjur Braendeland { 7915c9ac0cSSjur Braendeland struct sk_buff *skb; 8015c9ac0cSSjur Braendeland 8115c9ac0cSSjur Braendeland if (likely(in_interrupt())) 8215c9ac0cSSjur Braendeland skb = alloc_skb(len + pfx, GFP_ATOMIC); 8315c9ac0cSSjur Braendeland else 8415c9ac0cSSjur Braendeland skb = alloc_skb(len + pfx, GFP_KERNEL); 8515c9ac0cSSjur Braendeland 8615c9ac0cSSjur Braendeland if (unlikely(skb == NULL)) 8715c9ac0cSSjur Braendeland return NULL; 8815c9ac0cSSjur Braendeland 8915c9ac0cSSjur Braendeland skb_reserve(skb, pfx); 9015c9ac0cSSjur Braendeland return skb_to_pkt(skb); 9115c9ac0cSSjur Braendeland } 9215c9ac0cSSjur Braendeland 9315c9ac0cSSjur Braendeland inline struct cfpkt *cfpkt_create(u16 len) 9415c9ac0cSSjur Braendeland { 9515c9ac0cSSjur Braendeland return cfpkt_create_pfx(len + PKT_POSTFIX, PKT_PREFIX); 9615c9ac0cSSjur Braendeland } 9715c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_create); 9815c9ac0cSSjur Braendeland 9915c9ac0cSSjur Braendeland void cfpkt_destroy(struct cfpkt *pkt) 10015c9ac0cSSjur Braendeland { 10115c9ac0cSSjur Braendeland struct sk_buff *skb = pkt_to_skb(pkt); 10215c9ac0cSSjur Braendeland kfree_skb(skb); 10315c9ac0cSSjur Braendeland } 10415c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_destroy); 10515c9ac0cSSjur Braendeland 10615c9ac0cSSjur Braendeland inline bool cfpkt_more(struct cfpkt *pkt) 10715c9ac0cSSjur Braendeland { 10815c9ac0cSSjur Braendeland struct sk_buff *skb = pkt_to_skb(pkt); 10915c9ac0cSSjur Braendeland return skb->len > 0; 11015c9ac0cSSjur Braendeland } 11115c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_more); 11215c9ac0cSSjur Braendeland 11315c9ac0cSSjur Braendeland int cfpkt_peek_head(struct cfpkt *pkt, void *data, u16 len) 11415c9ac0cSSjur Braendeland { 11515c9ac0cSSjur Braendeland struct sk_buff *skb = pkt_to_skb(pkt); 11615c9ac0cSSjur Braendeland if (skb_headlen(skb) >= len) { 11715c9ac0cSSjur Braendeland memcpy(data, skb->data, len); 11815c9ac0cSSjur Braendeland return 0; 11915c9ac0cSSjur Braendeland } 12015c9ac0cSSjur Braendeland return !cfpkt_extr_head(pkt, data, len) && 12115c9ac0cSSjur Braendeland !cfpkt_add_head(pkt, data, len); 12215c9ac0cSSjur Braendeland } 12315c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_peek_head); 12415c9ac0cSSjur Braendeland 12515c9ac0cSSjur Braendeland int cfpkt_extr_head(struct cfpkt *pkt, void *data, u16 len) 12615c9ac0cSSjur Braendeland { 12715c9ac0cSSjur Braendeland struct sk_buff *skb = pkt_to_skb(pkt); 12815c9ac0cSSjur Braendeland u8 *from; 12915c9ac0cSSjur Braendeland if (unlikely(is_erronous(pkt))) 13015c9ac0cSSjur Braendeland return -EPROTO; 13115c9ac0cSSjur Braendeland 13215c9ac0cSSjur Braendeland if (unlikely(len > skb->len)) { 13315c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_extr_head read beyond end of packet\n"); 13415c9ac0cSSjur Braendeland return -EPROTO; 13515c9ac0cSSjur Braendeland } 13615c9ac0cSSjur Braendeland 13715c9ac0cSSjur Braendeland if (unlikely(len > skb_headlen(skb))) { 13815c9ac0cSSjur Braendeland if (unlikely(skb_linearize(skb) != 0)) { 13915c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_extr_head linearize failed\n"); 14015c9ac0cSSjur Braendeland return -EPROTO; 14115c9ac0cSSjur Braendeland } 14215c9ac0cSSjur Braendeland } 14315c9ac0cSSjur Braendeland from = skb_pull(skb, len); 14415c9ac0cSSjur Braendeland from -= len; 14515c9ac0cSSjur Braendeland memcpy(data, from, len); 14615c9ac0cSSjur Braendeland return 0; 14715c9ac0cSSjur Braendeland } 14815c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_extr_head); 14915c9ac0cSSjur Braendeland 15015c9ac0cSSjur Braendeland int cfpkt_extr_trail(struct cfpkt *pkt, void *dta, u16 len) 15115c9ac0cSSjur Braendeland { 15215c9ac0cSSjur Braendeland struct sk_buff *skb = pkt_to_skb(pkt); 15315c9ac0cSSjur Braendeland u8 *data = dta; 15415c9ac0cSSjur Braendeland u8 *from; 15515c9ac0cSSjur Braendeland if (unlikely(is_erronous(pkt))) 15615c9ac0cSSjur Braendeland return -EPROTO; 15715c9ac0cSSjur Braendeland 15815c9ac0cSSjur Braendeland if (unlikely(skb_linearize(skb) != 0)) { 15915c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_extr_trail linearize failed\n"); 16015c9ac0cSSjur Braendeland return -EPROTO; 16115c9ac0cSSjur Braendeland } 16215c9ac0cSSjur Braendeland if (unlikely(skb->data + len > skb_tail_pointer(skb))) { 16315c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_extr_trail read beyond end of packet\n"); 16415c9ac0cSSjur Braendeland return -EPROTO; 16515c9ac0cSSjur Braendeland } 16615c9ac0cSSjur Braendeland from = skb_tail_pointer(skb) - len; 16715c9ac0cSSjur Braendeland skb_trim(skb, skb->len - len); 16815c9ac0cSSjur Braendeland memcpy(data, from, len); 16915c9ac0cSSjur Braendeland return 0; 17015c9ac0cSSjur Braendeland } 17115c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_extr_trail); 17215c9ac0cSSjur Braendeland 17315c9ac0cSSjur Braendeland int cfpkt_pad_trail(struct cfpkt *pkt, u16 len) 17415c9ac0cSSjur Braendeland { 17515c9ac0cSSjur Braendeland return cfpkt_add_body(pkt, NULL, len); 17615c9ac0cSSjur Braendeland } 17715c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_pad_trail); 17815c9ac0cSSjur Braendeland 17915c9ac0cSSjur Braendeland int cfpkt_add_body(struct cfpkt *pkt, const void *data, u16 len) 18015c9ac0cSSjur Braendeland { 18115c9ac0cSSjur Braendeland struct sk_buff *skb = pkt_to_skb(pkt); 18215c9ac0cSSjur Braendeland struct sk_buff *lastskb; 18315c9ac0cSSjur Braendeland u8 *to; 18415c9ac0cSSjur Braendeland u16 addlen = 0; 18515c9ac0cSSjur Braendeland 18615c9ac0cSSjur Braendeland 18715c9ac0cSSjur Braendeland if (unlikely(is_erronous(pkt))) 18815c9ac0cSSjur Braendeland return -EPROTO; 18915c9ac0cSSjur Braendeland 19015c9ac0cSSjur Braendeland lastskb = skb; 19115c9ac0cSSjur Braendeland 19215c9ac0cSSjur Braendeland /* Check whether we need to add space at the tail */ 19315c9ac0cSSjur Braendeland if (unlikely(skb_tailroom(skb) < len)) { 19415c9ac0cSSjur Braendeland if (likely(len < PKT_LEN_WHEN_EXTENDING)) 19515c9ac0cSSjur Braendeland addlen = PKT_LEN_WHEN_EXTENDING; 19615c9ac0cSSjur Braendeland else 19715c9ac0cSSjur Braendeland addlen = len; 19815c9ac0cSSjur Braendeland } 19915c9ac0cSSjur Braendeland 20015c9ac0cSSjur Braendeland /* Check whether we need to change the SKB before writing to the tail */ 20115c9ac0cSSjur Braendeland if (unlikely((addlen > 0) || skb_cloned(skb) || skb_shared(skb))) { 20215c9ac0cSSjur Braendeland 20315c9ac0cSSjur Braendeland /* Make sure data is writable */ 20415c9ac0cSSjur Braendeland if (unlikely(skb_cow_data(skb, addlen, &lastskb) < 0)) { 20515c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_add_body: cow failed\n"); 20615c9ac0cSSjur Braendeland return -EPROTO; 20715c9ac0cSSjur Braendeland } 20815c9ac0cSSjur Braendeland /* 20915c9ac0cSSjur Braendeland * Is the SKB non-linear after skb_cow_data()? If so, we are 21015c9ac0cSSjur Braendeland * going to add data to the last SKB, so we need to adjust 21115c9ac0cSSjur Braendeland * lengths of the top SKB. 21215c9ac0cSSjur Braendeland */ 21315c9ac0cSSjur Braendeland if (lastskb != skb) { 21415c9ac0cSSjur Braendeland pr_warning("CAIF: %s(): Packet is non-linear\n", 21515c9ac0cSSjur Braendeland __func__); 21615c9ac0cSSjur Braendeland skb->len += len; 21715c9ac0cSSjur Braendeland skb->data_len += len; 21815c9ac0cSSjur Braendeland } 21915c9ac0cSSjur Braendeland } 22015c9ac0cSSjur Braendeland 22115c9ac0cSSjur Braendeland /* All set to put the last SKB and optionally write data there. */ 22215c9ac0cSSjur Braendeland to = skb_put(lastskb, len); 22315c9ac0cSSjur Braendeland if (likely(data)) 22415c9ac0cSSjur Braendeland memcpy(to, data, len); 22515c9ac0cSSjur Braendeland return 0; 22615c9ac0cSSjur Braendeland } 22715c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_add_body); 22815c9ac0cSSjur Braendeland 22915c9ac0cSSjur Braendeland inline int cfpkt_addbdy(struct cfpkt *pkt, u8 data) 23015c9ac0cSSjur Braendeland { 23115c9ac0cSSjur Braendeland return cfpkt_add_body(pkt, &data, 1); 23215c9ac0cSSjur Braendeland } 23315c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_addbdy); 23415c9ac0cSSjur Braendeland 23515c9ac0cSSjur Braendeland int cfpkt_add_head(struct cfpkt *pkt, const void *data2, u16 len) 23615c9ac0cSSjur Braendeland { 23715c9ac0cSSjur Braendeland struct sk_buff *skb = pkt_to_skb(pkt); 23815c9ac0cSSjur Braendeland struct sk_buff *lastskb; 23915c9ac0cSSjur Braendeland u8 *to; 24015c9ac0cSSjur Braendeland const u8 *data = data2; 241638e628aSSjur Braendeland int ret; 24215c9ac0cSSjur Braendeland if (unlikely(is_erronous(pkt))) 24315c9ac0cSSjur Braendeland return -EPROTO; 24415c9ac0cSSjur Braendeland if (unlikely(skb_headroom(skb) < len)) { 24515c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_add_head: no headroom\n"); 24615c9ac0cSSjur Braendeland return -EPROTO; 24715c9ac0cSSjur Braendeland } 24815c9ac0cSSjur Braendeland 24915c9ac0cSSjur Braendeland /* Make sure data is writable */ 250638e628aSSjur Braendeland ret = skb_cow_data(skb, 0, &lastskb); 251638e628aSSjur Braendeland if (unlikely(ret < 0)) { 25215c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_add_head: cow failed\n"); 253638e628aSSjur Braendeland return ret; 25415c9ac0cSSjur Braendeland } 25515c9ac0cSSjur Braendeland 25615c9ac0cSSjur Braendeland to = skb_push(skb, len); 25715c9ac0cSSjur Braendeland memcpy(to, data, len); 25815c9ac0cSSjur Braendeland return 0; 25915c9ac0cSSjur Braendeland } 26015c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_add_head); 26115c9ac0cSSjur Braendeland 26215c9ac0cSSjur Braendeland inline int cfpkt_add_trail(struct cfpkt *pkt, const void *data, u16 len) 26315c9ac0cSSjur Braendeland { 26415c9ac0cSSjur Braendeland return cfpkt_add_body(pkt, data, len); 26515c9ac0cSSjur Braendeland } 26615c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_add_trail); 26715c9ac0cSSjur Braendeland 26815c9ac0cSSjur Braendeland inline u16 cfpkt_getlen(struct cfpkt *pkt) 26915c9ac0cSSjur Braendeland { 27015c9ac0cSSjur Braendeland struct sk_buff *skb = pkt_to_skb(pkt); 27115c9ac0cSSjur Braendeland return skb->len; 27215c9ac0cSSjur Braendeland } 27315c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_getlen); 27415c9ac0cSSjur Braendeland 27515c9ac0cSSjur Braendeland inline u16 cfpkt_iterate(struct cfpkt *pkt, 27615c9ac0cSSjur Braendeland u16 (*iter_func)(u16, void *, u16), 27715c9ac0cSSjur Braendeland u16 data) 27815c9ac0cSSjur Braendeland { 27915c9ac0cSSjur Braendeland /* 28015c9ac0cSSjur Braendeland * Don't care about the performance hit of linearizing, 28115c9ac0cSSjur Braendeland * Checksum should not be used on high-speed interfaces anyway. 28215c9ac0cSSjur Braendeland */ 28315c9ac0cSSjur Braendeland if (unlikely(is_erronous(pkt))) 28415c9ac0cSSjur Braendeland return -EPROTO; 28515c9ac0cSSjur Braendeland if (unlikely(skb_linearize(&pkt->skb) != 0)) { 28615c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_iterate: linearize failed\n"); 28715c9ac0cSSjur Braendeland return -EPROTO; 28815c9ac0cSSjur Braendeland } 28915c9ac0cSSjur Braendeland return iter_func(data, pkt->skb.data, cfpkt_getlen(pkt)); 29015c9ac0cSSjur Braendeland } 29115c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_iterate); 29215c9ac0cSSjur Braendeland 29315c9ac0cSSjur Braendeland int cfpkt_setlen(struct cfpkt *pkt, u16 len) 29415c9ac0cSSjur Braendeland { 29515c9ac0cSSjur Braendeland struct sk_buff *skb = pkt_to_skb(pkt); 29615c9ac0cSSjur Braendeland 29715c9ac0cSSjur Braendeland 29815c9ac0cSSjur Braendeland if (unlikely(is_erronous(pkt))) 29915c9ac0cSSjur Braendeland return -EPROTO; 30015c9ac0cSSjur Braendeland 30115c9ac0cSSjur Braendeland if (likely(len <= skb->len)) { 30215c9ac0cSSjur Braendeland if (unlikely(skb->data_len)) 30315c9ac0cSSjur Braendeland ___pskb_trim(skb, len); 30415c9ac0cSSjur Braendeland else 30515c9ac0cSSjur Braendeland skb_trim(skb, len); 30615c9ac0cSSjur Braendeland 30715c9ac0cSSjur Braendeland return cfpkt_getlen(pkt); 30815c9ac0cSSjur Braendeland } 30915c9ac0cSSjur Braendeland 31015c9ac0cSSjur Braendeland /* Need to expand SKB */ 31115c9ac0cSSjur Braendeland if (unlikely(!cfpkt_pad_trail(pkt, len - skb->len))) 31215c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_setlen: skb_pad_trail failed\n"); 31315c9ac0cSSjur Braendeland 31415c9ac0cSSjur Braendeland return cfpkt_getlen(pkt); 31515c9ac0cSSjur Braendeland } 31615c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_setlen); 31715c9ac0cSSjur Braendeland 31815c9ac0cSSjur Braendeland struct cfpkt *cfpkt_create_uplink(const unsigned char *data, unsigned int len) 31915c9ac0cSSjur Braendeland { 32015c9ac0cSSjur Braendeland struct cfpkt *pkt = cfpkt_create_pfx(len + PKT_POSTFIX, PKT_PREFIX); 321638e628aSSjur Braendeland if (!pkt) 322638e628aSSjur Braendeland return NULL; 32315c9ac0cSSjur Braendeland if (unlikely(data != NULL)) 32415c9ac0cSSjur Braendeland cfpkt_add_body(pkt, data, len); 32515c9ac0cSSjur Braendeland return pkt; 32615c9ac0cSSjur Braendeland } 32715c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_create_uplink); 32815c9ac0cSSjur Braendeland 32915c9ac0cSSjur Braendeland struct cfpkt *cfpkt_append(struct cfpkt *dstpkt, 33015c9ac0cSSjur Braendeland struct cfpkt *addpkt, 33115c9ac0cSSjur Braendeland u16 expectlen) 33215c9ac0cSSjur Braendeland { 33315c9ac0cSSjur Braendeland struct sk_buff *dst = pkt_to_skb(dstpkt); 33415c9ac0cSSjur Braendeland struct sk_buff *add = pkt_to_skb(addpkt); 33515c9ac0cSSjur Braendeland u16 addlen = skb_headlen(add); 33615c9ac0cSSjur Braendeland u16 neededtailspace; 33715c9ac0cSSjur Braendeland struct sk_buff *tmp; 33815c9ac0cSSjur Braendeland u16 dstlen; 33915c9ac0cSSjur Braendeland u16 createlen; 34015c9ac0cSSjur Braendeland if (unlikely(is_erronous(dstpkt) || is_erronous(addpkt))) { 34115c9ac0cSSjur Braendeland return dstpkt; 34215c9ac0cSSjur Braendeland } 34315c9ac0cSSjur Braendeland if (expectlen > addlen) 34415c9ac0cSSjur Braendeland neededtailspace = expectlen; 34515c9ac0cSSjur Braendeland else 34615c9ac0cSSjur Braendeland neededtailspace = addlen; 34715c9ac0cSSjur Braendeland 34815c9ac0cSSjur Braendeland if (dst->tail + neededtailspace > dst->end) { 34915c9ac0cSSjur Braendeland /* Create a dumplicate of 'dst' with more tail space */ 350638e628aSSjur Braendeland struct cfpkt *tmppkt; 35115c9ac0cSSjur Braendeland dstlen = skb_headlen(dst); 35215c9ac0cSSjur Braendeland createlen = dstlen + neededtailspace; 353638e628aSSjur Braendeland tmppkt = cfpkt_create(createlen + PKT_PREFIX + PKT_POSTFIX); 354638e628aSSjur Braendeland if (tmppkt == NULL) 35515c9ac0cSSjur Braendeland return NULL; 356638e628aSSjur Braendeland tmp = pkt_to_skb(tmppkt); 35715c9ac0cSSjur Braendeland skb_set_tail_pointer(tmp, dstlen); 35815c9ac0cSSjur Braendeland tmp->len = dstlen; 35915c9ac0cSSjur Braendeland memcpy(tmp->data, dst->data, dstlen); 36015c9ac0cSSjur Braendeland cfpkt_destroy(dstpkt); 36115c9ac0cSSjur Braendeland dst = tmp; 36215c9ac0cSSjur Braendeland } 36315c9ac0cSSjur Braendeland memcpy(skb_tail_pointer(dst), add->data, skb_headlen(add)); 36415c9ac0cSSjur Braendeland cfpkt_destroy(addpkt); 36515c9ac0cSSjur Braendeland dst->tail += addlen; 36615c9ac0cSSjur Braendeland dst->len += addlen; 36715c9ac0cSSjur Braendeland return skb_to_pkt(dst); 36815c9ac0cSSjur Braendeland } 36915c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_append); 37015c9ac0cSSjur Braendeland 37115c9ac0cSSjur Braendeland struct cfpkt *cfpkt_split(struct cfpkt *pkt, u16 pos) 37215c9ac0cSSjur Braendeland { 37315c9ac0cSSjur Braendeland struct sk_buff *skb2; 37415c9ac0cSSjur Braendeland struct sk_buff *skb = pkt_to_skb(pkt); 375638e628aSSjur Braendeland struct cfpkt *tmppkt; 37615c9ac0cSSjur Braendeland u8 *split = skb->data + pos; 37715c9ac0cSSjur Braendeland u16 len2nd = skb_tail_pointer(skb) - split; 37815c9ac0cSSjur Braendeland 37915c9ac0cSSjur Braendeland if (unlikely(is_erronous(pkt))) 38015c9ac0cSSjur Braendeland return NULL; 38115c9ac0cSSjur Braendeland 38215c9ac0cSSjur Braendeland if (skb->data + pos > skb_tail_pointer(skb)) { 38315c9ac0cSSjur Braendeland PKT_ERROR(pkt, 38415c9ac0cSSjur Braendeland "cfpkt_split: trying to split beyond end of packet"); 38515c9ac0cSSjur Braendeland return NULL; 38615c9ac0cSSjur Braendeland } 38715c9ac0cSSjur Braendeland 38815c9ac0cSSjur Braendeland /* Create a new packet for the second part of the data */ 389638e628aSSjur Braendeland tmppkt = cfpkt_create_pfx(len2nd + PKT_PREFIX + PKT_POSTFIX, 390638e628aSSjur Braendeland PKT_PREFIX); 391638e628aSSjur Braendeland if (tmppkt == NULL) 392638e628aSSjur Braendeland return NULL; 393638e628aSSjur Braendeland skb2 = pkt_to_skb(tmppkt); 394638e628aSSjur Braendeland 39515c9ac0cSSjur Braendeland 39615c9ac0cSSjur Braendeland if (skb2 == NULL) 39715c9ac0cSSjur Braendeland return NULL; 39815c9ac0cSSjur Braendeland 39915c9ac0cSSjur Braendeland /* Reduce the length of the original packet */ 40015c9ac0cSSjur Braendeland skb_set_tail_pointer(skb, pos); 40115c9ac0cSSjur Braendeland skb->len = pos; 40215c9ac0cSSjur Braendeland 40315c9ac0cSSjur Braendeland memcpy(skb2->data, split, len2nd); 40415c9ac0cSSjur Braendeland skb2->tail += len2nd; 40515c9ac0cSSjur Braendeland skb2->len += len2nd; 40615c9ac0cSSjur Braendeland return skb_to_pkt(skb2); 40715c9ac0cSSjur Braendeland } 40815c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_split); 40915c9ac0cSSjur Braendeland 41015c9ac0cSSjur Braendeland char *cfpkt_log_pkt(struct cfpkt *pkt, char *buf, int buflen) 41115c9ac0cSSjur Braendeland { 41215c9ac0cSSjur Braendeland struct sk_buff *skb = pkt_to_skb(pkt); 41315c9ac0cSSjur Braendeland char *p = buf; 41415c9ac0cSSjur Braendeland int i; 41515c9ac0cSSjur Braendeland 41615c9ac0cSSjur Braendeland /* 41715c9ac0cSSjur Braendeland * Sanity check buffer length, it needs to be at least as large as 41815c9ac0cSSjur Braendeland * the header info: ~=50+ bytes 41915c9ac0cSSjur Braendeland */ 42015c9ac0cSSjur Braendeland if (buflen < 50) 42115c9ac0cSSjur Braendeland return NULL; 42215c9ac0cSSjur Braendeland 42315c9ac0cSSjur Braendeland snprintf(buf, buflen, "%s: pkt:%p len:%ld(%ld+%ld) {%ld,%ld} data: [", 42415c9ac0cSSjur Braendeland is_erronous(pkt) ? "ERRONOUS-SKB" : 42515c9ac0cSSjur Braendeland (skb->data_len != 0 ? "COMPLEX-SKB" : "SKB"), 42615c9ac0cSSjur Braendeland skb, 42715c9ac0cSSjur Braendeland (long) skb->len, 42815c9ac0cSSjur Braendeland (long) (skb_tail_pointer(skb) - skb->data), 42915c9ac0cSSjur Braendeland (long) skb->data_len, 43015c9ac0cSSjur Braendeland (long) (skb->data - skb->head), 43115c9ac0cSSjur Braendeland (long) (skb_tail_pointer(skb) - skb->head)); 43215c9ac0cSSjur Braendeland p = buf + strlen(buf); 43315c9ac0cSSjur Braendeland 43415c9ac0cSSjur Braendeland for (i = 0; i < skb_tail_pointer(skb) - skb->data && i < 300; i++) { 43515c9ac0cSSjur Braendeland if (p > buf + buflen - 10) { 43615c9ac0cSSjur Braendeland sprintf(p, "..."); 43715c9ac0cSSjur Braendeland p = buf + strlen(buf); 43815c9ac0cSSjur Braendeland break; 43915c9ac0cSSjur Braendeland } 44015c9ac0cSSjur Braendeland sprintf(p, "%02x,", skb->data[i]); 44115c9ac0cSSjur Braendeland p = buf + strlen(buf); 44215c9ac0cSSjur Braendeland } 44315c9ac0cSSjur Braendeland sprintf(p, "]\n"); 44415c9ac0cSSjur Braendeland return buf; 44515c9ac0cSSjur Braendeland } 44615c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_log_pkt); 44715c9ac0cSSjur Braendeland 44815c9ac0cSSjur Braendeland int cfpkt_raw_append(struct cfpkt *pkt, void **buf, unsigned int buflen) 44915c9ac0cSSjur Braendeland { 45015c9ac0cSSjur Braendeland struct sk_buff *skb = pkt_to_skb(pkt); 45115c9ac0cSSjur Braendeland struct sk_buff *lastskb; 45215c9ac0cSSjur Braendeland 45315c9ac0cSSjur Braendeland caif_assert(buf != NULL); 45415c9ac0cSSjur Braendeland if (unlikely(is_erronous(pkt))) 45515c9ac0cSSjur Braendeland return -EPROTO; 45615c9ac0cSSjur Braendeland /* Make sure SKB is writable */ 45715c9ac0cSSjur Braendeland if (unlikely(skb_cow_data(skb, 0, &lastskb) < 0)) { 45815c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_raw_append: skb_cow_data failed\n"); 45915c9ac0cSSjur Braendeland return -EPROTO; 46015c9ac0cSSjur Braendeland } 46115c9ac0cSSjur Braendeland 46215c9ac0cSSjur Braendeland if (unlikely(skb_linearize(skb) != 0)) { 46315c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_raw_append: linearize failed\n"); 46415c9ac0cSSjur Braendeland return -EPROTO; 46515c9ac0cSSjur Braendeland } 46615c9ac0cSSjur Braendeland 46715c9ac0cSSjur Braendeland if (unlikely(skb_tailroom(skb) < buflen)) { 46815c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_raw_append: buffer too short - failed\n"); 46915c9ac0cSSjur Braendeland return -EPROTO; 47015c9ac0cSSjur Braendeland } 47115c9ac0cSSjur Braendeland 47215c9ac0cSSjur Braendeland *buf = skb_put(skb, buflen); 47315c9ac0cSSjur Braendeland return 1; 47415c9ac0cSSjur Braendeland } 47515c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_raw_append); 47615c9ac0cSSjur Braendeland 47715c9ac0cSSjur Braendeland int cfpkt_raw_extract(struct cfpkt *pkt, void **buf, unsigned int buflen) 47815c9ac0cSSjur Braendeland { 47915c9ac0cSSjur Braendeland struct sk_buff *skb = pkt_to_skb(pkt); 48015c9ac0cSSjur Braendeland 48115c9ac0cSSjur Braendeland caif_assert(buf != NULL); 48215c9ac0cSSjur Braendeland if (unlikely(is_erronous(pkt))) 48315c9ac0cSSjur Braendeland return -EPROTO; 48415c9ac0cSSjur Braendeland 48515c9ac0cSSjur Braendeland if (unlikely(buflen > skb->len)) { 48615c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_raw_extract: buflen too large " 48715c9ac0cSSjur Braendeland "- failed\n"); 48815c9ac0cSSjur Braendeland return -EPROTO; 48915c9ac0cSSjur Braendeland } 49015c9ac0cSSjur Braendeland 49115c9ac0cSSjur Braendeland if (unlikely(buflen > skb_headlen(skb))) { 49215c9ac0cSSjur Braendeland if (unlikely(skb_linearize(skb) != 0)) { 49315c9ac0cSSjur Braendeland PKT_ERROR(pkt, "cfpkt_raw_extract: linearize failed\n"); 49415c9ac0cSSjur Braendeland return -EPROTO; 49515c9ac0cSSjur Braendeland } 49615c9ac0cSSjur Braendeland } 49715c9ac0cSSjur Braendeland 49815c9ac0cSSjur Braendeland *buf = skb->data; 49915c9ac0cSSjur Braendeland skb_pull(skb, buflen); 50015c9ac0cSSjur Braendeland 50115c9ac0cSSjur Braendeland return 1; 50215c9ac0cSSjur Braendeland } 50315c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_raw_extract); 50415c9ac0cSSjur Braendeland 50515c9ac0cSSjur Braendeland inline bool cfpkt_erroneous(struct cfpkt *pkt) 50615c9ac0cSSjur Braendeland { 50715c9ac0cSSjur Braendeland return cfpkt_priv(pkt)->erronous; 50815c9ac0cSSjur Braendeland } 50915c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_erroneous); 51015c9ac0cSSjur Braendeland 51115c9ac0cSSjur Braendeland struct cfpktq *cfpktq_create(void) 51215c9ac0cSSjur Braendeland { 51315c9ac0cSSjur Braendeland struct cfpktq *q = kmalloc(sizeof(struct cfpktq), GFP_ATOMIC); 51415c9ac0cSSjur Braendeland if (!q) 51515c9ac0cSSjur Braendeland return NULL; 51615c9ac0cSSjur Braendeland skb_queue_head_init(&q->head); 51715c9ac0cSSjur Braendeland atomic_set(&q->count, 0); 51815c9ac0cSSjur Braendeland spin_lock_init(&q->lock); 51915c9ac0cSSjur Braendeland return q; 52015c9ac0cSSjur Braendeland } 52115c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpktq_create); 52215c9ac0cSSjur Braendeland 52315c9ac0cSSjur Braendeland void cfpkt_queue(struct cfpktq *pktq, struct cfpkt *pkt, unsigned short prio) 52415c9ac0cSSjur Braendeland { 52515c9ac0cSSjur Braendeland atomic_inc(&pktq->count); 52615c9ac0cSSjur Braendeland spin_lock(&pktq->lock); 52715c9ac0cSSjur Braendeland skb_queue_tail(&pktq->head, pkt_to_skb(pkt)); 52815c9ac0cSSjur Braendeland spin_unlock(&pktq->lock); 52915c9ac0cSSjur Braendeland 53015c9ac0cSSjur Braendeland } 53115c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_queue); 53215c9ac0cSSjur Braendeland 53315c9ac0cSSjur Braendeland struct cfpkt *cfpkt_qpeek(struct cfpktq *pktq) 53415c9ac0cSSjur Braendeland { 53515c9ac0cSSjur Braendeland struct cfpkt *tmp; 53615c9ac0cSSjur Braendeland spin_lock(&pktq->lock); 53715c9ac0cSSjur Braendeland tmp = skb_to_pkt(skb_peek(&pktq->head)); 53815c9ac0cSSjur Braendeland spin_unlock(&pktq->lock); 53915c9ac0cSSjur Braendeland return tmp; 54015c9ac0cSSjur Braendeland } 54115c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_qpeek); 54215c9ac0cSSjur Braendeland 54315c9ac0cSSjur Braendeland struct cfpkt *cfpkt_dequeue(struct cfpktq *pktq) 54415c9ac0cSSjur Braendeland { 54515c9ac0cSSjur Braendeland struct cfpkt *pkt; 54615c9ac0cSSjur Braendeland spin_lock(&pktq->lock); 54715c9ac0cSSjur Braendeland pkt = skb_to_pkt(skb_dequeue(&pktq->head)); 54815c9ac0cSSjur Braendeland if (pkt) { 54915c9ac0cSSjur Braendeland atomic_dec(&pktq->count); 55015c9ac0cSSjur Braendeland caif_assert(atomic_read(&pktq->count) >= 0); 55115c9ac0cSSjur Braendeland } 55215c9ac0cSSjur Braendeland spin_unlock(&pktq->lock); 55315c9ac0cSSjur Braendeland return pkt; 55415c9ac0cSSjur Braendeland } 55515c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_dequeue); 55615c9ac0cSSjur Braendeland 55715c9ac0cSSjur Braendeland int cfpkt_qcount(struct cfpktq *pktq) 55815c9ac0cSSjur Braendeland { 55915c9ac0cSSjur Braendeland return atomic_read(&pktq->count); 56015c9ac0cSSjur Braendeland } 56115c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_qcount); 56215c9ac0cSSjur Braendeland 56315c9ac0cSSjur Braendeland struct cfpkt *cfpkt_clone_release(struct cfpkt *pkt) 56415c9ac0cSSjur Braendeland { 56515c9ac0cSSjur Braendeland struct cfpkt *clone; 56615c9ac0cSSjur Braendeland clone = skb_to_pkt(skb_clone(pkt_to_skb(pkt), GFP_ATOMIC)); 56715c9ac0cSSjur Braendeland /* Free original packet. */ 56815c9ac0cSSjur Braendeland cfpkt_destroy(pkt); 56915c9ac0cSSjur Braendeland if (!clone) 57015c9ac0cSSjur Braendeland return NULL; 57115c9ac0cSSjur Braendeland return clone; 57215c9ac0cSSjur Braendeland } 57315c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_clone_release); 57415c9ac0cSSjur Braendeland 57515c9ac0cSSjur Braendeland struct caif_payload_info *cfpkt_info(struct cfpkt *pkt) 57615c9ac0cSSjur Braendeland { 57715c9ac0cSSjur Braendeland return (struct caif_payload_info *)&pkt_to_skb(pkt)->cb; 57815c9ac0cSSjur Braendeland } 57915c9ac0cSSjur Braendeland EXPORT_SYMBOL(cfpkt_info); 580