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