1
2
3
4
5
6
7
8
9
10
11#include <linux/kernel.h>
12#include <linux/if_vlan.h>
13#include <linux/init.h>
14#include <linux/module.h>
15#include <linux/netlink.h>
16#include <linux/netfilter.h>
17#include <linux/netfilter/nf_tables.h>
18#include <net/netfilter/nf_tables_core.h>
19#include <net/netfilter/nf_tables.h>
20
21
22static bool
23nft_payload_copy_vlan(u32 *d, const struct sk_buff *skb, u8 offset, u8 len)
24{
25 int mac_off = skb_mac_header(skb) - skb->data;
26 u8 vlan_len, *vlanh, *dst_u8 = (u8 *) d;
27 struct vlan_ethhdr veth;
28
29 vlanh = (u8 *) &veth;
30 if (offset < ETH_HLEN) {
31 u8 ethlen = min_t(u8, len, ETH_HLEN - offset);
32
33 if (skb_copy_bits(skb, mac_off, &veth, ETH_HLEN))
34 return false;
35
36 veth.h_vlan_proto = skb->vlan_proto;
37
38 memcpy(dst_u8, vlanh + offset, ethlen);
39
40 len -= ethlen;
41 if (len == 0)
42 return true;
43
44 dst_u8 += ethlen;
45 offset = ETH_HLEN;
46 } else if (offset >= VLAN_ETH_HLEN) {
47 offset -= VLAN_HLEN;
48 goto skip;
49 }
50
51 veth.h_vlan_TCI = htons(skb_vlan_tag_get(skb));
52 veth.h_vlan_encapsulated_proto = skb->protocol;
53
54 vlanh += offset;
55
56 vlan_len = min_t(u8, len, VLAN_ETH_HLEN - offset);
57 memcpy(dst_u8, vlanh, vlan_len);
58
59 len -= vlan_len;
60 if (!len)
61 return true;
62
63 dst_u8 += vlan_len;
64 skip:
65 return skb_copy_bits(skb, offset + mac_off, dst_u8, len) == 0;
66}
67
68static void nft_payload_eval(const struct nft_expr *expr,
69 struct nft_regs *regs,
70 const struct nft_pktinfo *pkt)
71{
72 const struct nft_payload *priv = nft_expr_priv(expr);
73 const struct sk_buff *skb = pkt->skb;
74 u32 *dest = ®s->data[priv->dreg];
75 int offset;
76
77 dest[priv->len / NFT_REG32_SIZE] = 0;
78 switch (priv->base) {
79 case NFT_PAYLOAD_LL_HEADER:
80 if (!skb_mac_header_was_set(skb))
81 goto err;
82
83 if (skb_vlan_tag_present(skb)) {
84 if (!nft_payload_copy_vlan(dest, skb,
85 priv->offset, priv->len))
86 goto err;
87 return;
88 }
89 offset = skb_mac_header(skb) - skb->data;
90 break;
91 case NFT_PAYLOAD_NETWORK_HEADER:
92 offset = skb_network_offset(skb);
93 break;
94 case NFT_PAYLOAD_TRANSPORT_HEADER:
95 offset = pkt->xt.thoff;
96 break;
97 default:
98 BUG();
99 }
100 offset += priv->offset;
101
102 if (skb_copy_bits(skb, offset, dest, priv->len) < 0)
103 goto err;
104 return;
105err:
106 regs->verdict.code = NFT_BREAK;
107}
108
109static const struct nla_policy nft_payload_policy[NFTA_PAYLOAD_MAX + 1] = {
110 [NFTA_PAYLOAD_SREG] = { .type = NLA_U32 },
111 [NFTA_PAYLOAD_DREG] = { .type = NLA_U32 },
112 [NFTA_PAYLOAD_BASE] = { .type = NLA_U32 },
113 [NFTA_PAYLOAD_OFFSET] = { .type = NLA_U32 },
114 [NFTA_PAYLOAD_LEN] = { .type = NLA_U32 },
115 [NFTA_PAYLOAD_CSUM_TYPE] = { .type = NLA_U32 },
116 [NFTA_PAYLOAD_CSUM_OFFSET] = { .type = NLA_U32 },
117};
118
119static int nft_payload_init(const struct nft_ctx *ctx,
120 const struct nft_expr *expr,
121 const struct nlattr * const tb[])
122{
123 struct nft_payload *priv = nft_expr_priv(expr);
124
125 priv->base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
126 priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
127 priv->len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
128 priv->dreg = nft_parse_register(tb[NFTA_PAYLOAD_DREG]);
129
130 return nft_validate_register_store(ctx, priv->dreg, NULL,
131 NFT_DATA_VALUE, priv->len);
132}
133
134static int nft_payload_dump(struct sk_buff *skb, const struct nft_expr *expr)
135{
136 const struct nft_payload *priv = nft_expr_priv(expr);
137
138 if (nft_dump_register(skb, NFTA_PAYLOAD_DREG, priv->dreg) ||
139 nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) ||
140 nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) ||
141 nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)))
142 goto nla_put_failure;
143 return 0;
144
145nla_put_failure:
146 return -1;
147}
148
149static struct nft_expr_type nft_payload_type;
150static const struct nft_expr_ops nft_payload_ops = {
151 .type = &nft_payload_type,
152 .size = NFT_EXPR_SIZE(sizeof(struct nft_payload)),
153 .eval = nft_payload_eval,
154 .init = nft_payload_init,
155 .dump = nft_payload_dump,
156};
157
158const struct nft_expr_ops nft_payload_fast_ops = {
159 .type = &nft_payload_type,
160 .size = NFT_EXPR_SIZE(sizeof(struct nft_payload)),
161 .eval = nft_payload_eval,
162 .init = nft_payload_init,
163 .dump = nft_payload_dump,
164};
165
166static void nft_payload_set_eval(const struct nft_expr *expr,
167 struct nft_regs *regs,
168 const struct nft_pktinfo *pkt)
169{
170 const struct nft_payload_set *priv = nft_expr_priv(expr);
171 struct sk_buff *skb = pkt->skb;
172 const u32 *src = ®s->data[priv->sreg];
173 int offset, csum_offset;
174 __wsum fsum, tsum;
175 __sum16 sum;
176
177 switch (priv->base) {
178 case NFT_PAYLOAD_LL_HEADER:
179 if (!skb_mac_header_was_set(skb))
180 goto err;
181 offset = skb_mac_header(skb) - skb->data;
182 break;
183 case NFT_PAYLOAD_NETWORK_HEADER:
184 offset = skb_network_offset(skb);
185 break;
186 case NFT_PAYLOAD_TRANSPORT_HEADER:
187 offset = pkt->xt.thoff;
188 break;
189 default:
190 BUG();
191 }
192
193 csum_offset = offset + priv->csum_offset;
194 offset += priv->offset;
195
196 if (priv->csum_type == NFT_PAYLOAD_CSUM_INET &&
197 (priv->base != NFT_PAYLOAD_TRANSPORT_HEADER ||
198 skb->ip_summed != CHECKSUM_PARTIAL)) {
199 if (skb_copy_bits(skb, csum_offset, &sum, sizeof(sum)) < 0)
200 goto err;
201
202 fsum = skb_checksum(skb, offset, priv->len, 0);
203 tsum = csum_partial(src, priv->len, 0);
204 sum = csum_fold(csum_add(csum_sub(~csum_unfold(sum), fsum),
205 tsum));
206 if (sum == 0)
207 sum = CSUM_MANGLED_0;
208
209 if (!skb_make_writable(skb, csum_offset + sizeof(sum)) ||
210 skb_store_bits(skb, csum_offset, &sum, sizeof(sum)) < 0)
211 goto err;
212 }
213
214 if (!skb_make_writable(skb, max(offset + priv->len, 0)) ||
215 skb_store_bits(skb, offset, src, priv->len) < 0)
216 goto err;
217
218 return;
219err:
220 regs->verdict.code = NFT_BREAK;
221}
222
223static int nft_payload_set_init(const struct nft_ctx *ctx,
224 const struct nft_expr *expr,
225 const struct nlattr * const tb[])
226{
227 struct nft_payload_set *priv = nft_expr_priv(expr);
228
229 priv->base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
230 priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
231 priv->len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
232 priv->sreg = nft_parse_register(tb[NFTA_PAYLOAD_SREG]);
233
234 if (tb[NFTA_PAYLOAD_CSUM_TYPE])
235 priv->csum_type =
236 ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_TYPE]));
237 if (tb[NFTA_PAYLOAD_CSUM_OFFSET])
238 priv->csum_offset =
239 ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_OFFSET]));
240
241 switch (priv->csum_type) {
242 case NFT_PAYLOAD_CSUM_NONE:
243 case NFT_PAYLOAD_CSUM_INET:
244 break;
245 default:
246 return -EOPNOTSUPP;
247 }
248
249 return nft_validate_register_load(priv->sreg, priv->len);
250}
251
252static int nft_payload_set_dump(struct sk_buff *skb, const struct nft_expr *expr)
253{
254 const struct nft_payload_set *priv = nft_expr_priv(expr);
255
256 if (nft_dump_register(skb, NFTA_PAYLOAD_SREG, priv->sreg) ||
257 nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) ||
258 nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) ||
259 nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)) ||
260 nla_put_be32(skb, NFTA_PAYLOAD_CSUM_TYPE, htonl(priv->csum_type)) ||
261 nla_put_be32(skb, NFTA_PAYLOAD_CSUM_OFFSET,
262 htonl(priv->csum_offset)))
263 goto nla_put_failure;
264 return 0;
265
266nla_put_failure:
267 return -1;
268}
269
270static const struct nft_expr_ops nft_payload_set_ops = {
271 .type = &nft_payload_type,
272 .size = NFT_EXPR_SIZE(sizeof(struct nft_payload_set)),
273 .eval = nft_payload_set_eval,
274 .init = nft_payload_set_init,
275 .dump = nft_payload_set_dump,
276};
277
278static const struct nft_expr_ops *
279nft_payload_select_ops(const struct nft_ctx *ctx,
280 const struct nlattr * const tb[])
281{
282 enum nft_payload_bases base;
283 unsigned int offset, len;
284
285 if (tb[NFTA_PAYLOAD_BASE] == NULL ||
286 tb[NFTA_PAYLOAD_OFFSET] == NULL ||
287 tb[NFTA_PAYLOAD_LEN] == NULL)
288 return ERR_PTR(-EINVAL);
289
290 base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
291 switch (base) {
292 case NFT_PAYLOAD_LL_HEADER:
293 case NFT_PAYLOAD_NETWORK_HEADER:
294 case NFT_PAYLOAD_TRANSPORT_HEADER:
295 break;
296 default:
297 return ERR_PTR(-EOPNOTSUPP);
298 }
299
300 if (tb[NFTA_PAYLOAD_SREG] != NULL) {
301 if (tb[NFTA_PAYLOAD_DREG] != NULL)
302 return ERR_PTR(-EINVAL);
303 return &nft_payload_set_ops;
304 }
305
306 if (tb[NFTA_PAYLOAD_DREG] == NULL)
307 return ERR_PTR(-EINVAL);
308
309 offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
310 len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
311
312 if (len <= 4 && is_power_of_2(len) && IS_ALIGNED(offset, len) &&
313 base != NFT_PAYLOAD_LL_HEADER)
314 return &nft_payload_fast_ops;
315 else
316 return &nft_payload_ops;
317}
318
319static struct nft_expr_type nft_payload_type __read_mostly = {
320 .name = "payload",
321 .select_ops = nft_payload_select_ops,
322 .policy = nft_payload_policy,
323 .maxattr = NFTA_PAYLOAD_MAX,
324 .owner = THIS_MODULE,
325};
326
327int __init nft_payload_module_init(void)
328{
329 return nft_register_expr(&nft_payload_type);
330}
331
332void nft_payload_module_exit(void)
333{
334 nft_unregister_expr(&nft_payload_type);
335}
336
