1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * Copyright (c) 2008-2009 Patrick McHardy <kaber@trash.net>
4 * Copyright (c) 2016 Pablo Neira Ayuso <pablo@netfilter.org>
5 *
6 * Development of this code funded by Astaro AG (http://www.astaro.com/)
7 */
8
9 #include <linux/kernel.h>
10 #include <linux/if_vlan.h>
11 #include <linux/init.h>
12 #include <linux/module.h>
13 #include <linux/netlink.h>
14 #include <linux/netfilter.h>
15 #include <linux/netfilter/nf_tables.h>
16 #include <net/netfilter/nf_tables_core.h>
17 #include <net/netfilter/nf_tables.h>
18 #include <net/netfilter/nf_tables_offload.h>
19 /* For layer 4 checksum field offset. */
20 #include <linux/tcp.h>
21 #include <linux/udp.h>
22 #include <net/gre.h>
23 #include <linux/icmpv6.h>
24 #include <linux/ip.h>
25 #include <linux/ipv6.h>
26 #include <net/sctp/checksum.h>
27
nft_payload_rebuild_vlan_hdr(const struct sk_buff * skb,int mac_off,struct vlan_ethhdr * veth)28 static bool nft_payload_rebuild_vlan_hdr(const struct sk_buff *skb, int mac_off,
29 struct vlan_ethhdr *veth)
30 {
31 if (skb_copy_bits(skb, mac_off, veth, ETH_HLEN))
32 return false;
33
34 veth->h_vlan_proto = skb->vlan_proto;
35 veth->h_vlan_TCI = htons(skb_vlan_tag_get(skb));
36 veth->h_vlan_encapsulated_proto = skb->protocol;
37
38 return true;
39 }
40
41 /* add vlan header into the user buffer for if tag was removed by offloads */
42 static bool
nft_payload_copy_vlan(u32 * d,const struct sk_buff * skb,u16 offset,u8 len)43 nft_payload_copy_vlan(u32 *d, const struct sk_buff *skb, u16 offset, u8 len)
44 {
45 int mac_off = skb_mac_header(skb) - skb->data;
46 u8 *vlanh, *dst_u8 = (u8 *) d;
47 struct vlan_ethhdr veth;
48
49 vlanh = (u8 *) &veth;
50 if (offset < VLAN_ETH_HLEN) {
51 u8 ethlen = len;
52
53 if (!nft_payload_rebuild_vlan_hdr(skb, mac_off, &veth))
54 return false;
55
56 if (offset + len > VLAN_ETH_HLEN)
57 ethlen -= offset + len - VLAN_ETH_HLEN;
58
59 memcpy(dst_u8, vlanh + offset, ethlen);
60
61 len -= ethlen;
62 if (len == 0)
63 return true;
64
65 dst_u8 += ethlen;
66 offset = ETH_HLEN;
67 } else {
68 offset -= VLAN_HLEN;
69 }
70
71 return skb_copy_bits(skb, offset + mac_off, dst_u8, len) == 0;
72 }
73
__nft_payload_inner_offset(struct nft_pktinfo * pkt)74 static int __nft_payload_inner_offset(struct nft_pktinfo *pkt)
75 {
76 unsigned int thoff = nft_thoff(pkt);
77
78 if (!(pkt->flags & NFT_PKTINFO_L4PROTO) || pkt->fragoff)
79 return -1;
80
81 switch (pkt->tprot) {
82 case IPPROTO_UDP:
83 pkt->inneroff = thoff + sizeof(struct udphdr);
84 break;
85 case IPPROTO_TCP: {
86 struct tcphdr *th, _tcph;
87
88 th = skb_header_pointer(pkt->skb, thoff, sizeof(_tcph), &_tcph);
89 if (!th)
90 return -1;
91
92 pkt->inneroff = thoff + __tcp_hdrlen(th);
93 }
94 break;
95 case IPPROTO_GRE: {
96 u32 offset = sizeof(struct gre_base_hdr);
97 struct gre_base_hdr *gre, _gre;
98 __be16 version;
99
100 gre = skb_header_pointer(pkt->skb, thoff, sizeof(_gre), &_gre);
101 if (!gre)
102 return -1;
103
104 version = gre->flags & GRE_VERSION;
105 switch (version) {
106 case GRE_VERSION_0:
107 if (gre->flags & GRE_ROUTING)
108 return -1;
109
110 if (gre->flags & GRE_CSUM) {
111 offset += sizeof_field(struct gre_full_hdr, csum) +
112 sizeof_field(struct gre_full_hdr, reserved1);
113 }
114 if (gre->flags & GRE_KEY)
115 offset += sizeof_field(struct gre_full_hdr, key);
116
117 if (gre->flags & GRE_SEQ)
118 offset += sizeof_field(struct gre_full_hdr, seq);
119 break;
120 default:
121 return -1;
122 }
123
124 pkt->inneroff = thoff + offset;
125 }
126 break;
127 case IPPROTO_IPIP:
128 pkt->inneroff = thoff;
129 break;
130 default:
131 return -1;
132 }
133
134 pkt->flags |= NFT_PKTINFO_INNER;
135
136 return 0;
137 }
138
nft_payload_inner_offset(const struct nft_pktinfo * pkt)139 int nft_payload_inner_offset(const struct nft_pktinfo *pkt)
140 {
141 if (!(pkt->flags & NFT_PKTINFO_INNER) &&
142 __nft_payload_inner_offset((struct nft_pktinfo *)pkt) < 0)
143 return -1;
144
145 return pkt->inneroff;
146 }
147
nft_payload_need_vlan_adjust(u32 offset,u32 len)148 static bool nft_payload_need_vlan_adjust(u32 offset, u32 len)
149 {
150 unsigned int boundary = offset + len;
151
152 /* data past ether src/dst requested, copy needed */
153 if (boundary > offsetof(struct ethhdr, h_proto))
154 return true;
155
156 return false;
157 }
158
nft_payload_eval(const struct nft_expr * expr,struct nft_regs * regs,const struct nft_pktinfo * pkt)159 void nft_payload_eval(const struct nft_expr *expr,
160 struct nft_regs *regs,
161 const struct nft_pktinfo *pkt)
162 {
163 const struct nft_payload *priv = nft_expr_priv(expr);
164 const struct sk_buff *skb = pkt->skb;
165 u32 *dest = ®s->data[priv->dreg];
166 int offset;
167
168 if (priv->len % NFT_REG32_SIZE)
169 dest[priv->len / NFT_REG32_SIZE] = 0;
170
171 switch (priv->base) {
172 case NFT_PAYLOAD_LL_HEADER:
173 if (!skb_mac_header_was_set(skb) || skb_mac_header_len(skb) == 0)
174 goto err;
175
176 if (skb_vlan_tag_present(skb) &&
177 nft_payload_need_vlan_adjust(priv->offset, priv->len)) {
178 if (!nft_payload_copy_vlan(dest, skb,
179 priv->offset, priv->len))
180 goto err;
181 return;
182 }
183 offset = skb_mac_header(skb) - skb->data;
184 break;
185 case NFT_PAYLOAD_NETWORK_HEADER:
186 offset = skb_network_offset(skb) + pkt->nhoff;
187 break;
188 case NFT_PAYLOAD_TRANSPORT_HEADER:
189 if (!(pkt->flags & NFT_PKTINFO_L4PROTO) || pkt->fragoff)
190 goto err;
191 offset = nft_thoff(pkt);
192 break;
193 case NFT_PAYLOAD_INNER_HEADER:
194 offset = nft_payload_inner_offset(pkt);
195 if (offset < 0)
196 goto err;
197 break;
198 default:
199 WARN_ON_ONCE(1);
200 goto err;
201 }
202 offset += priv->offset;
203
204 if (skb_copy_bits(skb, offset, dest, priv->len) < 0)
205 goto err;
206 return;
207 err:
208 regs->verdict.code = NFT_BREAK;
209 }
210
211 static const struct nla_policy nft_payload_policy[NFTA_PAYLOAD_MAX + 1] = {
212 [NFTA_PAYLOAD_SREG] = NLA_POLICY_MAX(NLA_BE32, NFT_REG32_MAX),
213 [NFTA_PAYLOAD_DREG] = NLA_POLICY_MAX(NLA_BE32, NFT_REG32_MAX),
214 [NFTA_PAYLOAD_BASE] = { .type = NLA_U32 },
215 [NFTA_PAYLOAD_OFFSET] = { .type = NLA_BE32 },
216 [NFTA_PAYLOAD_LEN] = NLA_POLICY_MAX(NLA_BE32, 255),
217 [NFTA_PAYLOAD_CSUM_TYPE] = { .type = NLA_U32 },
218 [NFTA_PAYLOAD_CSUM_OFFSET] = NLA_POLICY_MAX(NLA_BE32, 255),
219 [NFTA_PAYLOAD_CSUM_FLAGS] = NLA_POLICY_MASK(NLA_BE32, NFT_PAYLOAD_L4CSUM_PSEUDOHDR),
220 };
221
nft_payload_init(const struct nft_ctx * ctx,const struct nft_expr * expr,const struct nlattr * const tb[])222 static int nft_payload_init(const struct nft_ctx *ctx,
223 const struct nft_expr *expr,
224 const struct nlattr * const tb[])
225 {
226 struct nft_payload *priv = nft_expr_priv(expr);
227
228 priv->base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
229 priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
230 priv->len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
231
232 return nft_parse_register_store(ctx, tb[NFTA_PAYLOAD_DREG],
233 &priv->dreg, NULL, NFT_DATA_VALUE,
234 priv->len);
235 }
236
nft_payload_dump(struct sk_buff * skb,const struct nft_expr * expr,bool reset)237 static int nft_payload_dump(struct sk_buff *skb,
238 const struct nft_expr *expr, bool reset)
239 {
240 const struct nft_payload *priv = nft_expr_priv(expr);
241
242 if (nft_dump_register(skb, NFTA_PAYLOAD_DREG, priv->dreg) ||
243 nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) ||
244 nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) ||
245 nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)))
246 goto nla_put_failure;
247 return 0;
248
249 nla_put_failure:
250 return -1;
251 }
252
nft_payload_offload_mask(struct nft_offload_reg * reg,u32 priv_len,u32 field_len)253 static bool nft_payload_offload_mask(struct nft_offload_reg *reg,
254 u32 priv_len, u32 field_len)
255 {
256 unsigned int remainder, delta, k;
257 struct nft_data mask = {};
258 __be32 remainder_mask;
259
260 if (priv_len == field_len) {
261 memset(®->mask, 0xff, priv_len);
262 return true;
263 } else if (priv_len > field_len) {
264 return false;
265 }
266
267 memset(&mask, 0xff, field_len);
268 remainder = priv_len % sizeof(u32);
269 if (remainder) {
270 k = priv_len / sizeof(u32);
271 delta = field_len - priv_len;
272 remainder_mask = htonl(~((1 << (delta * BITS_PER_BYTE)) - 1));
273 mask.data[k] = (__force u32)remainder_mask;
274 }
275
276 memcpy(®->mask, &mask, field_len);
277
278 return true;
279 }
280
nft_payload_offload_ll(struct nft_offload_ctx * ctx,struct nft_flow_rule * flow,const struct nft_payload * priv)281 static int nft_payload_offload_ll(struct nft_offload_ctx *ctx,
282 struct nft_flow_rule *flow,
283 const struct nft_payload *priv)
284 {
285 struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
286
287 switch (priv->offset) {
288 case offsetof(struct ethhdr, h_source):
289 if (!nft_payload_offload_mask(reg, priv->len, ETH_ALEN))
290 return -EOPNOTSUPP;
291
292 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_ETH_ADDRS, eth_addrs,
293 src, ETH_ALEN, reg);
294 break;
295 case offsetof(struct ethhdr, h_dest):
296 if (!nft_payload_offload_mask(reg, priv->len, ETH_ALEN))
297 return -EOPNOTSUPP;
298
299 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_ETH_ADDRS, eth_addrs,
300 dst, ETH_ALEN, reg);
301 break;
302 case offsetof(struct ethhdr, h_proto):
303 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
304 return -EOPNOTSUPP;
305
306 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_BASIC, basic,
307 n_proto, sizeof(__be16), reg);
308 nft_offload_set_dependency(ctx, NFT_OFFLOAD_DEP_NETWORK);
309 break;
310 case offsetof(struct vlan_ethhdr, h_vlan_TCI):
311 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
312 return -EOPNOTSUPP;
313
314 NFT_OFFLOAD_MATCH_FLAGS(FLOW_DISSECTOR_KEY_VLAN, vlan,
315 vlan_tci, sizeof(__be16), reg,
316 NFT_OFFLOAD_F_NETWORK2HOST);
317 break;
318 case offsetof(struct vlan_ethhdr, h_vlan_encapsulated_proto):
319 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
320 return -EOPNOTSUPP;
321
322 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_VLAN, vlan,
323 vlan_tpid, sizeof(__be16), reg);
324 nft_offload_set_dependency(ctx, NFT_OFFLOAD_DEP_NETWORK);
325 break;
326 case offsetof(struct vlan_ethhdr, h_vlan_TCI) + sizeof(struct vlan_hdr):
327 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
328 return -EOPNOTSUPP;
329
330 NFT_OFFLOAD_MATCH_FLAGS(FLOW_DISSECTOR_KEY_CVLAN, cvlan,
331 vlan_tci, sizeof(__be16), reg,
332 NFT_OFFLOAD_F_NETWORK2HOST);
333 break;
334 case offsetof(struct vlan_ethhdr, h_vlan_encapsulated_proto) +
335 sizeof(struct vlan_hdr):
336 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
337 return -EOPNOTSUPP;
338
339 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_CVLAN, cvlan,
340 vlan_tpid, sizeof(__be16), reg);
341 nft_offload_set_dependency(ctx, NFT_OFFLOAD_DEP_NETWORK);
342 break;
343 default:
344 return -EOPNOTSUPP;
345 }
346
347 return 0;
348 }
349
nft_payload_offload_ip(struct nft_offload_ctx * ctx,struct nft_flow_rule * flow,const struct nft_payload * priv)350 static int nft_payload_offload_ip(struct nft_offload_ctx *ctx,
351 struct nft_flow_rule *flow,
352 const struct nft_payload *priv)
353 {
354 struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
355
356 switch (priv->offset) {
357 case offsetof(struct iphdr, saddr):
358 if (!nft_payload_offload_mask(reg, priv->len,
359 sizeof(struct in_addr)))
360 return -EOPNOTSUPP;
361
362 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_IPV4_ADDRS, ipv4, src,
363 sizeof(struct in_addr), reg);
364 nft_flow_rule_set_addr_type(flow, FLOW_DISSECTOR_KEY_IPV4_ADDRS);
365 break;
366 case offsetof(struct iphdr, daddr):
367 if (!nft_payload_offload_mask(reg, priv->len,
368 sizeof(struct in_addr)))
369 return -EOPNOTSUPP;
370
371 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_IPV4_ADDRS, ipv4, dst,
372 sizeof(struct in_addr), reg);
373 nft_flow_rule_set_addr_type(flow, FLOW_DISSECTOR_KEY_IPV4_ADDRS);
374 break;
375 case offsetof(struct iphdr, protocol):
376 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__u8)))
377 return -EOPNOTSUPP;
378
379 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_BASIC, basic, ip_proto,
380 sizeof(__u8), reg);
381 nft_offload_set_dependency(ctx, NFT_OFFLOAD_DEP_TRANSPORT);
382 break;
383 default:
384 return -EOPNOTSUPP;
385 }
386
387 return 0;
388 }
389
nft_payload_offload_ip6(struct nft_offload_ctx * ctx,struct nft_flow_rule * flow,const struct nft_payload * priv)390 static int nft_payload_offload_ip6(struct nft_offload_ctx *ctx,
391 struct nft_flow_rule *flow,
392 const struct nft_payload *priv)
393 {
394 struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
395
396 switch (priv->offset) {
397 case offsetof(struct ipv6hdr, saddr):
398 if (!nft_payload_offload_mask(reg, priv->len,
399 sizeof(struct in6_addr)))
400 return -EOPNOTSUPP;
401
402 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_IPV6_ADDRS, ipv6, src,
403 sizeof(struct in6_addr), reg);
404 nft_flow_rule_set_addr_type(flow, FLOW_DISSECTOR_KEY_IPV6_ADDRS);
405 break;
406 case offsetof(struct ipv6hdr, daddr):
407 if (!nft_payload_offload_mask(reg, priv->len,
408 sizeof(struct in6_addr)))
409 return -EOPNOTSUPP;
410
411 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_IPV6_ADDRS, ipv6, dst,
412 sizeof(struct in6_addr), reg);
413 nft_flow_rule_set_addr_type(flow, FLOW_DISSECTOR_KEY_IPV6_ADDRS);
414 break;
415 case offsetof(struct ipv6hdr, nexthdr):
416 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__u8)))
417 return -EOPNOTSUPP;
418
419 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_BASIC, basic, ip_proto,
420 sizeof(__u8), reg);
421 nft_offload_set_dependency(ctx, NFT_OFFLOAD_DEP_TRANSPORT);
422 break;
423 default:
424 return -EOPNOTSUPP;
425 }
426
427 return 0;
428 }
429
nft_payload_offload_nh(struct nft_offload_ctx * ctx,struct nft_flow_rule * flow,const struct nft_payload * priv)430 static int nft_payload_offload_nh(struct nft_offload_ctx *ctx,
431 struct nft_flow_rule *flow,
432 const struct nft_payload *priv)
433 {
434 int err;
435
436 switch (ctx->dep.l3num) {
437 case htons(ETH_P_IP):
438 err = nft_payload_offload_ip(ctx, flow, priv);
439 break;
440 case htons(ETH_P_IPV6):
441 err = nft_payload_offload_ip6(ctx, flow, priv);
442 break;
443 default:
444 return -EOPNOTSUPP;
445 }
446
447 return err;
448 }
449
nft_payload_offload_tcp(struct nft_offload_ctx * ctx,struct nft_flow_rule * flow,const struct nft_payload * priv)450 static int nft_payload_offload_tcp(struct nft_offload_ctx *ctx,
451 struct nft_flow_rule *flow,
452 const struct nft_payload *priv)
453 {
454 struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
455
456 switch (priv->offset) {
457 case offsetof(struct tcphdr, source):
458 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
459 return -EOPNOTSUPP;
460
461 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_PORTS, tp, src,
462 sizeof(__be16), reg);
463 break;
464 case offsetof(struct tcphdr, dest):
465 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
466 return -EOPNOTSUPP;
467
468 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_PORTS, tp, dst,
469 sizeof(__be16), reg);
470 break;
471 default:
472 return -EOPNOTSUPP;
473 }
474
475 return 0;
476 }
477
nft_payload_offload_udp(struct nft_offload_ctx * ctx,struct nft_flow_rule * flow,const struct nft_payload * priv)478 static int nft_payload_offload_udp(struct nft_offload_ctx *ctx,
479 struct nft_flow_rule *flow,
480 const struct nft_payload *priv)
481 {
482 struct nft_offload_reg *reg = &ctx->regs[priv->dreg];
483
484 switch (priv->offset) {
485 case offsetof(struct udphdr, source):
486 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
487 return -EOPNOTSUPP;
488
489 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_PORTS, tp, src,
490 sizeof(__be16), reg);
491 break;
492 case offsetof(struct udphdr, dest):
493 if (!nft_payload_offload_mask(reg, priv->len, sizeof(__be16)))
494 return -EOPNOTSUPP;
495
496 NFT_OFFLOAD_MATCH(FLOW_DISSECTOR_KEY_PORTS, tp, dst,
497 sizeof(__be16), reg);
498 break;
499 default:
500 return -EOPNOTSUPP;
501 }
502
503 return 0;
504 }
505
nft_payload_offload_th(struct nft_offload_ctx * ctx,struct nft_flow_rule * flow,const struct nft_payload * priv)506 static int nft_payload_offload_th(struct nft_offload_ctx *ctx,
507 struct nft_flow_rule *flow,
508 const struct nft_payload *priv)
509 {
510 int err;
511
512 switch (ctx->dep.protonum) {
513 case IPPROTO_TCP:
514 err = nft_payload_offload_tcp(ctx, flow, priv);
515 break;
516 case IPPROTO_UDP:
517 err = nft_payload_offload_udp(ctx, flow, priv);
518 break;
519 default:
520 return -EOPNOTSUPP;
521 }
522
523 return err;
524 }
525
nft_payload_offload(struct nft_offload_ctx * ctx,struct nft_flow_rule * flow,const struct nft_expr * expr)526 static int nft_payload_offload(struct nft_offload_ctx *ctx,
527 struct nft_flow_rule *flow,
528 const struct nft_expr *expr)
529 {
530 const struct nft_payload *priv = nft_expr_priv(expr);
531 int err;
532
533 switch (priv->base) {
534 case NFT_PAYLOAD_LL_HEADER:
535 err = nft_payload_offload_ll(ctx, flow, priv);
536 break;
537 case NFT_PAYLOAD_NETWORK_HEADER:
538 err = nft_payload_offload_nh(ctx, flow, priv);
539 break;
540 case NFT_PAYLOAD_TRANSPORT_HEADER:
541 err = nft_payload_offload_th(ctx, flow, priv);
542 break;
543 default:
544 err = -EOPNOTSUPP;
545 break;
546 }
547 return err;
548 }
549
550 static const struct nft_expr_ops nft_payload_ops = {
551 .type = &nft_payload_type,
552 .size = NFT_EXPR_SIZE(sizeof(struct nft_payload)),
553 .eval = nft_payload_eval,
554 .init = nft_payload_init,
555 .dump = nft_payload_dump,
556 .offload = nft_payload_offload,
557 };
558
559 const struct nft_expr_ops nft_payload_fast_ops = {
560 .type = &nft_payload_type,
561 .size = NFT_EXPR_SIZE(sizeof(struct nft_payload)),
562 .eval = nft_payload_eval,
563 .init = nft_payload_init,
564 .dump = nft_payload_dump,
565 .offload = nft_payload_offload,
566 };
567
nft_payload_inner_eval(const struct nft_expr * expr,struct nft_regs * regs,const struct nft_pktinfo * pkt,struct nft_inner_tun_ctx * tun_ctx)568 void nft_payload_inner_eval(const struct nft_expr *expr, struct nft_regs *regs,
569 const struct nft_pktinfo *pkt,
570 struct nft_inner_tun_ctx *tun_ctx)
571 {
572 const struct nft_payload *priv = nft_expr_priv(expr);
573 const struct sk_buff *skb = pkt->skb;
574 u32 *dest = ®s->data[priv->dreg];
575 int offset;
576
577 if (priv->len % NFT_REG32_SIZE)
578 dest[priv->len / NFT_REG32_SIZE] = 0;
579
580 switch (priv->base) {
581 case NFT_PAYLOAD_TUN_HEADER:
582 if (!(tun_ctx->flags & NFT_PAYLOAD_CTX_INNER_TUN))
583 goto err;
584
585 offset = tun_ctx->inner_tunoff;
586 break;
587 case NFT_PAYLOAD_LL_HEADER:
588 if (!(tun_ctx->flags & NFT_PAYLOAD_CTX_INNER_LL))
589 goto err;
590
591 offset = tun_ctx->inner_lloff;
592 break;
593 case NFT_PAYLOAD_NETWORK_HEADER:
594 if (!(tun_ctx->flags & NFT_PAYLOAD_CTX_INNER_NH))
595 goto err;
596
597 offset = tun_ctx->inner_nhoff;
598 break;
599 case NFT_PAYLOAD_TRANSPORT_HEADER:
600 if (!(tun_ctx->flags & NFT_PAYLOAD_CTX_INNER_TH))
601 goto err;
602
603 offset = tun_ctx->inner_thoff;
604 break;
605 default:
606 WARN_ON_ONCE(1);
607 goto err;
608 }
609 offset += priv->offset;
610
611 if (skb_copy_bits(skb, offset, dest, priv->len) < 0)
612 goto err;
613
614 return;
615 err:
616 regs->verdict.code = NFT_BREAK;
617 }
618
nft_payload_inner_init(const struct nft_ctx * ctx,const struct nft_expr * expr,const struct nlattr * const tb[])619 static int nft_payload_inner_init(const struct nft_ctx *ctx,
620 const struct nft_expr *expr,
621 const struct nlattr * const tb[])
622 {
623 struct nft_payload *priv = nft_expr_priv(expr);
624 u32 base;
625
626 if (!tb[NFTA_PAYLOAD_BASE] || !tb[NFTA_PAYLOAD_OFFSET] ||
627 !tb[NFTA_PAYLOAD_LEN] || !tb[NFTA_PAYLOAD_DREG])
628 return -EINVAL;
629
630 base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
631 switch (base) {
632 case NFT_PAYLOAD_TUN_HEADER:
633 case NFT_PAYLOAD_LL_HEADER:
634 case NFT_PAYLOAD_NETWORK_HEADER:
635 case NFT_PAYLOAD_TRANSPORT_HEADER:
636 break;
637 default:
638 return -EOPNOTSUPP;
639 }
640
641 priv->base = base;
642 priv->offset = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_OFFSET]));
643 priv->len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
644
645 return nft_parse_register_store(ctx, tb[NFTA_PAYLOAD_DREG],
646 &priv->dreg, NULL, NFT_DATA_VALUE,
647 priv->len);
648 }
649
650 static const struct nft_expr_ops nft_payload_inner_ops = {
651 .type = &nft_payload_type,
652 .size = NFT_EXPR_SIZE(sizeof(struct nft_payload)),
653 .init = nft_payload_inner_init,
654 .dump = nft_payload_dump,
655 /* direct call to nft_payload_inner_eval(). */
656 };
657
nft_csum_replace(__sum16 * sum,__wsum fsum,__wsum tsum)658 static inline void nft_csum_replace(__sum16 *sum, __wsum fsum, __wsum tsum)
659 {
660 csum_replace4(sum, (__force __be32)fsum, (__force __be32)tsum);
661 if (*sum == 0)
662 *sum = CSUM_MANGLED_0;
663 }
664
nft_payload_udp_checksum(struct sk_buff * skb,unsigned int thoff)665 static bool nft_payload_udp_checksum(struct sk_buff *skb, unsigned int thoff)
666 {
667 struct udphdr *uh, _uh;
668
669 uh = skb_header_pointer(skb, thoff, sizeof(_uh), &_uh);
670 if (!uh)
671 return false;
672
673 return (__force bool)uh->check;
674 }
675
nft_payload_l4csum_offset(const struct nft_pktinfo * pkt,struct sk_buff * skb,unsigned int * l4csum_offset)676 static int nft_payload_l4csum_offset(const struct nft_pktinfo *pkt,
677 struct sk_buff *skb,
678 unsigned int *l4csum_offset)
679 {
680 if (pkt->fragoff)
681 return -1;
682
683 switch (pkt->tprot) {
684 case IPPROTO_TCP:
685 *l4csum_offset = offsetof(struct tcphdr, check);
686 break;
687 case IPPROTO_UDP:
688 if (!nft_payload_udp_checksum(skb, nft_thoff(pkt)))
689 return -1;
690 fallthrough;
691 case IPPROTO_UDPLITE:
692 *l4csum_offset = offsetof(struct udphdr, check);
693 break;
694 case IPPROTO_ICMPV6:
695 *l4csum_offset = offsetof(struct icmp6hdr, icmp6_cksum);
696 break;
697 default:
698 return -1;
699 }
700
701 *l4csum_offset += nft_thoff(pkt);
702 return 0;
703 }
704
nft_payload_csum_sctp(struct sk_buff * skb,int offset)705 static int nft_payload_csum_sctp(struct sk_buff *skb, int offset)
706 {
707 struct sctphdr *sh;
708
709 if (skb_ensure_writable(skb, offset + sizeof(*sh)))
710 return -1;
711
712 sh = (struct sctphdr *)(skb->data + offset);
713 sh->checksum = sctp_compute_cksum(skb, offset);
714 skb->ip_summed = CHECKSUM_UNNECESSARY;
715 return 0;
716 }
717
nft_payload_l4csum_update(const struct nft_pktinfo * pkt,struct sk_buff * skb,__wsum fsum,__wsum tsum)718 static int nft_payload_l4csum_update(const struct nft_pktinfo *pkt,
719 struct sk_buff *skb,
720 __wsum fsum, __wsum tsum)
721 {
722 int l4csum_offset;
723 __sum16 sum;
724
725 /* If we cannot determine layer 4 checksum offset or this packet doesn't
726 * require layer 4 checksum recalculation, skip this packet.
727 */
728 if (nft_payload_l4csum_offset(pkt, skb, &l4csum_offset) < 0)
729 return 0;
730
731 if (skb_copy_bits(skb, l4csum_offset, &sum, sizeof(sum)) < 0)
732 return -1;
733
734 /* Checksum mangling for an arbitrary amount of bytes, based on
735 * inet_proto_csum_replace*() functions.
736 */
737 if (skb->ip_summed != CHECKSUM_PARTIAL) {
738 nft_csum_replace(&sum, fsum, tsum);
739 if (skb->ip_summed == CHECKSUM_COMPLETE) {
740 skb->csum = ~csum_add(csum_sub(~(skb->csum), fsum),
741 tsum);
742 }
743 } else {
744 sum = ~csum_fold(csum_add(csum_sub(csum_unfold(sum), fsum),
745 tsum));
746 }
747
748 if (skb_ensure_writable(skb, l4csum_offset + sizeof(sum)) ||
749 skb_store_bits(skb, l4csum_offset, &sum, sizeof(sum)) < 0)
750 return -1;
751
752 return 0;
753 }
754
nft_payload_csum_inet(struct sk_buff * skb,const u32 * src,__wsum fsum,__wsum tsum,int csum_offset)755 static int nft_payload_csum_inet(struct sk_buff *skb, const u32 *src,
756 __wsum fsum, __wsum tsum, int csum_offset)
757 {
758 __sum16 sum;
759
760 if (skb_copy_bits(skb, csum_offset, &sum, sizeof(sum)) < 0)
761 return -1;
762
763 nft_csum_replace(&sum, fsum, tsum);
764 if (skb_ensure_writable(skb, csum_offset + sizeof(sum)) ||
765 skb_store_bits(skb, csum_offset, &sum, sizeof(sum)) < 0)
766 return -1;
767
768 return 0;
769 }
770
771 struct nft_payload_set {
772 enum nft_payload_bases base:8;
773 u16 offset;
774 u8 len;
775 u8 sreg;
776 u8 csum_type;
777 u8 csum_offset;
778 u8 csum_flags;
779 };
780
781 /* This is not struct vlan_hdr. */
782 struct nft_payload_vlan_hdr {
783 __be16 h_vlan_proto;
784 __be16 h_vlan_TCI;
785 };
786
787 static bool
nft_payload_set_vlan(const u32 * src,struct sk_buff * skb,u16 offset,u8 len,int * vlan_hlen)788 nft_payload_set_vlan(const u32 *src, struct sk_buff *skb, u16 offset, u8 len,
789 int *vlan_hlen)
790 {
791 struct nft_payload_vlan_hdr *vlanh;
792 __be16 vlan_proto;
793 u16 vlan_tci;
794
795 if (offset >= offsetof(struct vlan_ethhdr, h_vlan_encapsulated_proto)) {
796 *vlan_hlen = VLAN_HLEN;
797 return true;
798 }
799
800 switch (offset) {
801 case offsetof(struct vlan_ethhdr, h_vlan_proto):
802 if (len == 2) {
803 vlan_proto = nft_reg_load_be16(src);
804 skb->vlan_proto = vlan_proto;
805 } else if (len == 4) {
806 vlanh = (struct nft_payload_vlan_hdr *)src;
807 __vlan_hwaccel_put_tag(skb, vlanh->h_vlan_proto,
808 ntohs(vlanh->h_vlan_TCI));
809 } else {
810 return false;
811 }
812 break;
813 case offsetof(struct vlan_ethhdr, h_vlan_TCI):
814 if (len != 2)
815 return false;
816
817 vlan_tci = ntohs(nft_reg_load_be16(src));
818 skb->vlan_tci = vlan_tci;
819 break;
820 default:
821 return false;
822 }
823
824 return true;
825 }
826
nft_payload_set_eval(const struct nft_expr * expr,struct nft_regs * regs,const struct nft_pktinfo * pkt)827 static void nft_payload_set_eval(const struct nft_expr *expr,
828 struct nft_regs *regs,
829 const struct nft_pktinfo *pkt)
830 {
831 const struct nft_payload_set *priv = nft_expr_priv(expr);
832 const u32 *src = ®s->data[priv->sreg];
833 int offset, csum_offset, vlan_hlen = 0;
834 struct sk_buff *skb = pkt->skb;
835 __wsum fsum, tsum;
836
837 switch (priv->base) {
838 case NFT_PAYLOAD_LL_HEADER:
839 if (!skb_mac_header_was_set(skb))
840 goto err;
841
842 if (skb_vlan_tag_present(skb) &&
843 nft_payload_need_vlan_adjust(priv->offset, priv->len)) {
844 if (!nft_payload_set_vlan(src, skb,
845 priv->offset, priv->len,
846 &vlan_hlen))
847 goto err;
848
849 if (!vlan_hlen)
850 return;
851 }
852
853 offset = skb_mac_header(skb) - skb->data - vlan_hlen;
854 break;
855 case NFT_PAYLOAD_NETWORK_HEADER:
856 offset = skb_network_offset(skb);
857 break;
858 case NFT_PAYLOAD_TRANSPORT_HEADER:
859 if (!(pkt->flags & NFT_PKTINFO_L4PROTO) || pkt->fragoff)
860 goto err;
861 offset = nft_thoff(pkt);
862 break;
863 case NFT_PAYLOAD_INNER_HEADER:
864 offset = nft_payload_inner_offset(pkt);
865 if (offset < 0)
866 goto err;
867 break;
868 default:
869 WARN_ON_ONCE(1);
870 goto err;
871 }
872
873 csum_offset = offset + priv->csum_offset;
874 offset += priv->offset;
875
876 if ((priv->csum_type == NFT_PAYLOAD_CSUM_INET || priv->csum_flags) &&
877 ((priv->base != NFT_PAYLOAD_TRANSPORT_HEADER &&
878 priv->base != NFT_PAYLOAD_INNER_HEADER) ||
879 skb->ip_summed != CHECKSUM_PARTIAL)) {
880 if (offset + priv->len > skb->len)
881 goto err;
882
883 fsum = skb_checksum(skb, offset, priv->len, 0);
884 tsum = csum_partial(src, priv->len, 0);
885
886 if (priv->csum_type == NFT_PAYLOAD_CSUM_INET &&
887 nft_payload_csum_inet(skb, src, fsum, tsum, csum_offset))
888 goto err;
889
890 if (priv->csum_flags &&
891 nft_payload_l4csum_update(pkt, skb, fsum, tsum) < 0)
892 goto err;
893 }
894
895 if (skb_ensure_writable(skb, max(offset + priv->len, 0)) ||
896 skb_store_bits(skb, offset, src, priv->len) < 0)
897 goto err;
898
899 if (priv->csum_type == NFT_PAYLOAD_CSUM_SCTP &&
900 pkt->tprot == IPPROTO_SCTP &&
901 skb->ip_summed != CHECKSUM_PARTIAL) {
902 if (pkt->fragoff == 0 &&
903 nft_payload_csum_sctp(skb, nft_thoff(pkt)))
904 goto err;
905 }
906
907 return;
908 err:
909 regs->verdict.code = NFT_BREAK;
910 }
911
nft_payload_set_init(const struct nft_ctx * ctx,const struct nft_expr * expr,const struct nlattr * const tb[])912 static int nft_payload_set_init(const struct nft_ctx *ctx,
913 const struct nft_expr *expr,
914 const struct nlattr * const tb[])
915 {
916 u32 csum_offset, offset, csum_type = NFT_PAYLOAD_CSUM_NONE;
917 struct nft_payload_set *priv = nft_expr_priv(expr);
918 int err;
919
920 if (ctx->net->user_ns != &init_user_ns)
921 return -EPERM;
922
923 priv->base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
924 priv->len = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_LEN]));
925
926 err = nft_parse_u32_check(tb[NFTA_PAYLOAD_OFFSET], U16_MAX, &offset);
927 if (err < 0)
928 return err;
929 priv->offset = offset;
930
931 if (tb[NFTA_PAYLOAD_CSUM_TYPE])
932 csum_type = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_TYPE]));
933 if (tb[NFTA_PAYLOAD_CSUM_OFFSET]) {
934 err = nft_parse_u32_check(tb[NFTA_PAYLOAD_CSUM_OFFSET], U8_MAX,
935 &csum_offset);
936 if (err < 0)
937 return err;
938
939 priv->csum_offset = csum_offset;
940 }
941 if (tb[NFTA_PAYLOAD_CSUM_FLAGS]) {
942 u32 flags;
943
944 flags = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_CSUM_FLAGS]));
945 if (flags & ~NFT_PAYLOAD_L4CSUM_PSEUDOHDR)
946 return -EINVAL;
947
948 priv->csum_flags = flags;
949 }
950
951 switch (csum_type) {
952 case NFT_PAYLOAD_CSUM_NONE:
953 case NFT_PAYLOAD_CSUM_INET:
954 break;
955 case NFT_PAYLOAD_CSUM_SCTP:
956 if (priv->base != NFT_PAYLOAD_TRANSPORT_HEADER)
957 return -EINVAL;
958
959 if (priv->csum_offset != offsetof(struct sctphdr, checksum))
960 return -EINVAL;
961 break;
962 default:
963 return -EOPNOTSUPP;
964 }
965 priv->csum_type = csum_type;
966
967 return nft_parse_register_load(ctx, tb[NFTA_PAYLOAD_SREG], &priv->sreg,
968 priv->len);
969 }
970
nft_payload_set_dump(struct sk_buff * skb,const struct nft_expr * expr,bool reset)971 static int nft_payload_set_dump(struct sk_buff *skb,
972 const struct nft_expr *expr, bool reset)
973 {
974 const struct nft_payload_set *priv = nft_expr_priv(expr);
975
976 if (nft_dump_register(skb, NFTA_PAYLOAD_SREG, priv->sreg) ||
977 nla_put_be32(skb, NFTA_PAYLOAD_BASE, htonl(priv->base)) ||
978 nla_put_be32(skb, NFTA_PAYLOAD_OFFSET, htonl(priv->offset)) ||
979 nla_put_be32(skb, NFTA_PAYLOAD_LEN, htonl(priv->len)) ||
980 nla_put_be32(skb, NFTA_PAYLOAD_CSUM_TYPE, htonl(priv->csum_type)) ||
981 nla_put_be32(skb, NFTA_PAYLOAD_CSUM_OFFSET,
982 htonl(priv->csum_offset)) ||
983 nla_put_be32(skb, NFTA_PAYLOAD_CSUM_FLAGS, htonl(priv->csum_flags)))
984 goto nla_put_failure;
985 return 0;
986
987 nla_put_failure:
988 return -1;
989 }
990
991 static const struct nft_expr_ops nft_payload_set_ops = {
992 .type = &nft_payload_type,
993 .size = NFT_EXPR_SIZE(sizeof(struct nft_payload_set)),
994 .eval = nft_payload_set_eval,
995 .init = nft_payload_set_init,
996 .dump = nft_payload_set_dump,
997 };
998
999 static const struct nft_expr_ops *
nft_payload_select_ops(const struct nft_ctx * ctx,const struct nlattr * const tb[])1000 nft_payload_select_ops(const struct nft_ctx *ctx,
1001 const struct nlattr * const tb[])
1002 {
1003 enum nft_payload_bases base;
1004 unsigned int offset, len;
1005 int err;
1006
1007 if (tb[NFTA_PAYLOAD_BASE] == NULL ||
1008 tb[NFTA_PAYLOAD_OFFSET] == NULL ||
1009 tb[NFTA_PAYLOAD_LEN] == NULL)
1010 return ERR_PTR(-EINVAL);
1011
1012 base = ntohl(nla_get_be32(tb[NFTA_PAYLOAD_BASE]));
1013 switch (base) {
1014 case NFT_PAYLOAD_LL_HEADER:
1015 case NFT_PAYLOAD_NETWORK_HEADER:
1016 case NFT_PAYLOAD_TRANSPORT_HEADER:
1017 case NFT_PAYLOAD_INNER_HEADER:
1018 break;
1019 default:
1020 return ERR_PTR(-EOPNOTSUPP);
1021 }
1022
1023 if (tb[NFTA_PAYLOAD_SREG] != NULL) {
1024 if (tb[NFTA_PAYLOAD_DREG] != NULL)
1025 return ERR_PTR(-EINVAL);
1026 return &nft_payload_set_ops;
1027 }
1028
1029 if (tb[NFTA_PAYLOAD_DREG] == NULL)
1030 return ERR_PTR(-EINVAL);
1031
1032 err = nft_parse_u32_check(tb[NFTA_PAYLOAD_OFFSET], U16_MAX, &offset);
1033 if (err < 0)
1034 return ERR_PTR(err);
1035
1036 err = nft_parse_u32_check(tb[NFTA_PAYLOAD_LEN], U8_MAX, &len);
1037 if (err < 0)
1038 return ERR_PTR(err);
1039
1040 if (len <= 4 && is_power_of_2(len) && IS_ALIGNED(offset, len) &&
1041 base != NFT_PAYLOAD_LL_HEADER && base != NFT_PAYLOAD_INNER_HEADER)
1042 return &nft_payload_fast_ops;
1043 else
1044 return &nft_payload_ops;
1045 }
1046
1047 struct nft_expr_type nft_payload_type __read_mostly = {
1048 .name = "payload",
1049 .select_ops = nft_payload_select_ops,
1050 .inner_ops = &nft_payload_inner_ops,
1051 .policy = nft_payload_policy,
1052 .maxattr = NFTA_PAYLOAD_MAX,
1053 .owner = THIS_MODULE,
1054 };
1055