1 | /* |
2 | * net/ife/ife.c - Inter-FE protocol based on ForCES WG InterFE LFB |
3 | * Copyright (c) 2015 Jamal Hadi Salim <jhs@mojatatu.com> |
4 | * Copyright (c) 2017 Yotam Gigi <yotamg@mellanox.com> |
5 | * |
6 | * Refer to: draft-ietf-forces-interfelfb-03 and netdev01 paper: |
7 | * "Distributing Linux Traffic Control Classifier-Action Subsystem" |
8 | * Authors: Jamal Hadi Salim and Damascene M. Joachimpillai |
9 | * |
10 | * This program is free software; you can redistribute it and/or modify |
11 | * it under the terms of the GNU General Public License as published by |
12 | * the Free Software Foundation. |
13 | */ |
14 | |
15 | #include <linux/types.h> |
16 | #include <linux/kernel.h> |
17 | #include <linux/string.h> |
18 | #include <linux/errno.h> |
19 | #include <linux/skbuff.h> |
20 | #include <linux/rtnetlink.h> |
21 | #include <linux/module.h> |
22 | #include <linux/init.h> |
23 | #include <net/net_namespace.h> |
24 | #include <net/netlink.h> |
25 | #include <net/pkt_sched.h> |
26 | #include <linux/etherdevice.h> |
27 | #include <net/ife.h> |
28 | |
29 | struct ifeheadr { |
30 | __be16 metalen; |
31 | u8 tlv_data[]; |
32 | }; |
33 | |
34 | void *ife_encode(struct sk_buff *skb, u16 metalen) |
35 | { |
36 | /* OUTERHDR:TOTMETALEN:{TLVHDR:Metadatum:TLVHDR..}:ORIGDATA |
37 | * where ORIGDATA = original ethernet header ... |
38 | */ |
39 | int hdrm = metalen + IFE_METAHDRLEN; |
40 | int total_push = hdrm + skb->dev->hard_header_len; |
41 | struct ifeheadr *ifehdr; |
42 | struct ethhdr *iethh; /* inner ether header */ |
43 | int skboff = 0; |
44 | int err; |
45 | |
46 | err = skb_cow_head(skb, headroom: total_push); |
47 | if (unlikely(err)) |
48 | return NULL; |
49 | |
50 | iethh = (struct ethhdr *) skb->data; |
51 | |
52 | __skb_push(skb, len: total_push); |
53 | memcpy(skb->data, iethh, skb->dev->hard_header_len); |
54 | skb_reset_mac_header(skb); |
55 | skboff += skb->dev->hard_header_len; |
56 | |
57 | /* total metadata length */ |
58 | ifehdr = (struct ifeheadr *) (skb->data + skboff); |
59 | metalen += IFE_METAHDRLEN; |
60 | ifehdr->metalen = htons(metalen); |
61 | |
62 | return ifehdr->tlv_data; |
63 | } |
64 | EXPORT_SYMBOL_GPL(ife_encode); |
65 | |
66 | void *ife_decode(struct sk_buff *skb, u16 *metalen) |
67 | { |
68 | struct ifeheadr *ifehdr; |
69 | int total_pull; |
70 | u16 ifehdrln; |
71 | |
72 | if (!pskb_may_pull(skb, len: skb->dev->hard_header_len + IFE_METAHDRLEN)) |
73 | return NULL; |
74 | |
75 | ifehdr = (struct ifeheadr *) (skb->data + skb->dev->hard_header_len); |
76 | ifehdrln = ntohs(ifehdr->metalen); |
77 | total_pull = skb->dev->hard_header_len + ifehdrln; |
78 | |
79 | if (unlikely(ifehdrln < 2)) |
80 | return NULL; |
81 | |
82 | if (unlikely(!pskb_may_pull(skb, total_pull))) |
83 | return NULL; |
84 | |
85 | skb_set_mac_header(skb, offset: total_pull); |
86 | __skb_pull(skb, len: total_pull); |
87 | *metalen = ifehdrln - IFE_METAHDRLEN; |
88 | |
89 | return &ifehdr->tlv_data; |
90 | } |
91 | EXPORT_SYMBOL_GPL(ife_decode); |
92 | |
93 | struct meta_tlvhdr { |
94 | __be16 type; |
95 | __be16 len; |
96 | }; |
97 | |
98 | static bool __ife_tlv_meta_valid(const unsigned char *skbdata, |
99 | const unsigned char *ifehdr_end) |
100 | { |
101 | const struct meta_tlvhdr *tlv; |
102 | u16 tlvlen; |
103 | |
104 | if (unlikely(skbdata + sizeof(*tlv) > ifehdr_end)) |
105 | return false; |
106 | |
107 | tlv = (const struct meta_tlvhdr *)skbdata; |
108 | tlvlen = ntohs(tlv->len); |
109 | |
110 | /* tlv length field is inc header, check on minimum */ |
111 | if (tlvlen < NLA_HDRLEN) |
112 | return false; |
113 | |
114 | /* overflow by NLA_ALIGN check */ |
115 | if (NLA_ALIGN(tlvlen) < tlvlen) |
116 | return false; |
117 | |
118 | if (unlikely(skbdata + NLA_ALIGN(tlvlen) > ifehdr_end)) |
119 | return false; |
120 | |
121 | return true; |
122 | } |
123 | |
124 | /* Caller takes care of presenting data in network order |
125 | */ |
126 | void *ife_tlv_meta_decode(void *skbdata, const void *ifehdr_end, u16 *attrtype, |
127 | u16 *dlen, u16 *totlen) |
128 | { |
129 | struct meta_tlvhdr *tlv; |
130 | |
131 | if (!__ife_tlv_meta_valid(skbdata, ifehdr_end)) |
132 | return NULL; |
133 | |
134 | tlv = (struct meta_tlvhdr *)skbdata; |
135 | *dlen = ntohs(tlv->len) - NLA_HDRLEN; |
136 | *attrtype = ntohs(tlv->type); |
137 | |
138 | if (totlen) |
139 | *totlen = nla_total_size(payload: *dlen); |
140 | |
141 | return skbdata + sizeof(struct meta_tlvhdr); |
142 | } |
143 | EXPORT_SYMBOL_GPL(ife_tlv_meta_decode); |
144 | |
145 | void *ife_tlv_meta_next(void *skbdata) |
146 | { |
147 | struct meta_tlvhdr *tlv = (struct meta_tlvhdr *) skbdata; |
148 | u16 tlvlen = ntohs(tlv->len); |
149 | |
150 | tlvlen = NLA_ALIGN(tlvlen); |
151 | |
152 | return skbdata + tlvlen; |
153 | } |
154 | EXPORT_SYMBOL_GPL(ife_tlv_meta_next); |
155 | |
156 | /* Caller takes care of presenting data in network order |
157 | */ |
158 | int ife_tlv_meta_encode(void *skbdata, u16 attrtype, u16 dlen, const void *dval) |
159 | { |
160 | __be32 *tlv = (__be32 *) (skbdata); |
161 | u16 totlen = nla_total_size(payload: dlen); /*alignment + hdr */ |
162 | char *dptr = (char *) tlv + NLA_HDRLEN; |
163 | u32 htlv = attrtype << 16 | (dlen + NLA_HDRLEN); |
164 | |
165 | *tlv = htonl(htlv); |
166 | memset(dptr, 0, totlen - NLA_HDRLEN); |
167 | memcpy(dptr, dval, dlen); |
168 | |
169 | return totlen; |
170 | } |
171 | EXPORT_SYMBOL_GPL(ife_tlv_meta_encode); |
172 | |
173 | MODULE_AUTHOR("Jamal Hadi Salim <jhs@mojatatu.com>" ); |
174 | MODULE_AUTHOR("Yotam Gigi <yotam.gi@gmail.com>" ); |
175 | MODULE_DESCRIPTION("Inter-FE LFB action" ); |
176 | MODULE_LICENSE("GPL" ); |
177 | |