1
2
3
4
5
6
7
8#include <asm/unaligned.h>
9#include <linux/kernel.h>
10#include <linux/netlink.h>
11#include <linux/netfilter.h>
12#include <linux/netfilter/nf_tables.h>
13#include <linux/sctp.h>
14#include <net/netfilter/nf_tables_core.h>
15#include <net/netfilter/nf_tables.h>
16#include <net/sctp/sctp.h>
17#include <net/tcp.h>
18
19struct nft_exthdr {
20 u8 type;
21 u8 offset;
22 u8 len;
23 u8 op;
24 u8 dreg;
25 u8 sreg;
26 u8 flags;
27};
28
29static unsigned int optlen(const u8 *opt, unsigned int offset)
30{
31
32 if (opt[offset] <= TCPOPT_NOP || opt[offset + 1] == 0)
33 return 1;
34 else
35 return opt[offset + 1];
36}
37
38static void nft_exthdr_ipv6_eval(const struct nft_expr *expr,
39 struct nft_regs *regs,
40 const struct nft_pktinfo *pkt)
41{
42 struct nft_exthdr *priv = nft_expr_priv(expr);
43 u32 *dest = ®s->data[priv->dreg];
44 unsigned int offset = 0;
45 int err;
46
47 if (pkt->skb->protocol != htons(ETH_P_IPV6))
48 goto err;
49
50 err = ipv6_find_hdr(pkt->skb, &offset, priv->type, NULL, NULL);
51 if (priv->flags & NFT_EXTHDR_F_PRESENT) {
52 nft_reg_store8(dest, err >= 0);
53 return;
54 } else if (err < 0) {
55 goto err;
56 }
57 offset += priv->offset;
58
59 dest[priv->len / NFT_REG32_SIZE] = 0;
60 if (skb_copy_bits(pkt->skb, offset, dest, priv->len) < 0)
61 goto err;
62 return;
63err:
64 regs->verdict.code = NFT_BREAK;
65}
66
67
68
69
70
71
72
73
74
75static int ipv4_find_option(struct net *net, struct sk_buff *skb,
76 unsigned int *offset, int target)
77{
78 unsigned char optbuf[sizeof(struct ip_options) + 40];
79 struct ip_options *opt = (struct ip_options *)optbuf;
80 struct iphdr *iph, _iph;
81 unsigned int start;
82 bool found = false;
83 __be32 info;
84 int optlen;
85
86 iph = skb_header_pointer(skb, 0, sizeof(_iph), &_iph);
87 if (!iph)
88 return -EBADMSG;
89 start = sizeof(struct iphdr);
90
91 optlen = iph->ihl * 4 - (int)sizeof(struct iphdr);
92 if (optlen <= 0)
93 return -ENOENT;
94
95 memset(opt, 0, sizeof(struct ip_options));
96
97
98
99 if (skb_copy_bits(skb, start, opt->__data, optlen))
100 return -EBADMSG;
101 opt->optlen = optlen;
102
103 if (__ip_options_compile(net, opt, NULL, &info))
104 return -EBADMSG;
105
106 switch (target) {
107 case IPOPT_SSRR:
108 case IPOPT_LSRR:
109 if (!opt->srr)
110 break;
111 found = target == IPOPT_SSRR ? opt->is_strictroute :
112 !opt->is_strictroute;
113 if (found)
114 *offset = opt->srr + start;
115 break;
116 case IPOPT_RR:
117 if (!opt->rr)
118 break;
119 *offset = opt->rr + start;
120 found = true;
121 break;
122 case IPOPT_RA:
123 if (!opt->router_alert)
124 break;
125 *offset = opt->router_alert + start;
126 found = true;
127 break;
128 default:
129 return -EOPNOTSUPP;
130 }
131 return found ? target : -ENOENT;
132}
133
134static void nft_exthdr_ipv4_eval(const struct nft_expr *expr,
135 struct nft_regs *regs,
136 const struct nft_pktinfo *pkt)
137{
138 struct nft_exthdr *priv = nft_expr_priv(expr);
139 u32 *dest = ®s->data[priv->dreg];
140 struct sk_buff *skb = pkt->skb;
141 unsigned int offset;
142 int err;
143
144 if (skb->protocol != htons(ETH_P_IP))
145 goto err;
146
147 err = ipv4_find_option(nft_net(pkt), skb, &offset, priv->type);
148 if (priv->flags & NFT_EXTHDR_F_PRESENT) {
149 nft_reg_store8(dest, err >= 0);
150 return;
151 } else if (err < 0) {
152 goto err;
153 }
154 offset += priv->offset;
155
156 dest[priv->len / NFT_REG32_SIZE] = 0;
157 if (skb_copy_bits(pkt->skb, offset, dest, priv->len) < 0)
158 goto err;
159 return;
160err:
161 regs->verdict.code = NFT_BREAK;
162}
163
164static void *
165nft_tcp_header_pointer(const struct nft_pktinfo *pkt,
166 unsigned int len, void *buffer, unsigned int *tcphdr_len)
167{
168 struct tcphdr *tcph;
169
170 if (pkt->tprot != IPPROTO_TCP || pkt->fragoff)
171 return NULL;
172
173 tcph = skb_header_pointer(pkt->skb, nft_thoff(pkt), sizeof(*tcph), buffer);
174 if (!tcph)
175 return NULL;
176
177 *tcphdr_len = __tcp_hdrlen(tcph);
178 if (*tcphdr_len < sizeof(*tcph) || *tcphdr_len > len)
179 return NULL;
180
181 return skb_header_pointer(pkt->skb, nft_thoff(pkt), *tcphdr_len, buffer);
182}
183
184static void nft_exthdr_tcp_eval(const struct nft_expr *expr,
185 struct nft_regs *regs,
186 const struct nft_pktinfo *pkt)
187{
188 u8 buff[sizeof(struct tcphdr) + MAX_TCP_OPTION_SPACE];
189 struct nft_exthdr *priv = nft_expr_priv(expr);
190 unsigned int i, optl, tcphdr_len, offset;
191 u32 *dest = ®s->data[priv->dreg];
192 struct tcphdr *tcph;
193 u8 *opt;
194
195 tcph = nft_tcp_header_pointer(pkt, sizeof(buff), buff, &tcphdr_len);
196 if (!tcph)
197 goto err;
198
199 opt = (u8 *)tcph;
200 for (i = sizeof(*tcph); i < tcphdr_len - 1; i += optl) {
201 optl = optlen(opt, i);
202
203 if (priv->type != opt[i])
204 continue;
205
206 if (i + optl > tcphdr_len || priv->len + priv->offset > optl)
207 goto err;
208
209 offset = i + priv->offset;
210 if (priv->flags & NFT_EXTHDR_F_PRESENT) {
211 *dest = 1;
212 } else {
213 dest[priv->len / NFT_REG32_SIZE] = 0;
214 memcpy(dest, opt + offset, priv->len);
215 }
216
217 return;
218 }
219
220err:
221 if (priv->flags & NFT_EXTHDR_F_PRESENT)
222 *dest = 0;
223 else
224 regs->verdict.code = NFT_BREAK;
225}
226
227static void nft_exthdr_tcp_set_eval(const struct nft_expr *expr,
228 struct nft_regs *regs,
229 const struct nft_pktinfo *pkt)
230{
231 u8 buff[sizeof(struct tcphdr) + MAX_TCP_OPTION_SPACE];
232 struct nft_exthdr *priv = nft_expr_priv(expr);
233 unsigned int i, optl, tcphdr_len, offset;
234 struct tcphdr *tcph;
235 u8 *opt;
236
237 tcph = nft_tcp_header_pointer(pkt, sizeof(buff), buff, &tcphdr_len);
238 if (!tcph)
239 goto err;
240
241 opt = (u8 *)tcph;
242 for (i = sizeof(*tcph); i < tcphdr_len - 1; i += optl) {
243 union {
244 __be16 v16;
245 __be32 v32;
246 } old, new;
247
248 optl = optlen(opt, i);
249
250 if (priv->type != opt[i])
251 continue;
252
253 if (i + optl > tcphdr_len || priv->len + priv->offset > optl)
254 goto err;
255
256 if (skb_ensure_writable(pkt->skb,
257 nft_thoff(pkt) + i + priv->len))
258 goto err;
259
260 tcph = nft_tcp_header_pointer(pkt, sizeof(buff), buff,
261 &tcphdr_len);
262 if (!tcph)
263 goto err;
264
265 offset = i + priv->offset;
266
267 switch (priv->len) {
268 case 2:
269 old.v16 = get_unaligned((u16 *)(opt + offset));
270 new.v16 = (__force __be16)nft_reg_load16(
271 ®s->data[priv->sreg]);
272
273 switch (priv->type) {
274 case TCPOPT_MSS:
275
276 if (ntohs(old.v16) <= ntohs(new.v16))
277 return;
278 break;
279 }
280
281 if (old.v16 == new.v16)
282 return;
283
284 put_unaligned(new.v16, (u16*)(opt + offset));
285 inet_proto_csum_replace2(&tcph->check, pkt->skb,
286 old.v16, new.v16, false);
287 break;
288 case 4:
289 new.v32 = regs->data[priv->sreg];
290 old.v32 = get_unaligned((u32 *)(opt + offset));
291
292 if (old.v32 == new.v32)
293 return;
294
295 put_unaligned(new.v32, (u32*)(opt + offset));
296 inet_proto_csum_replace4(&tcph->check, pkt->skb,
297 old.v32, new.v32, false);
298 break;
299 default:
300 WARN_ON_ONCE(1);
301 break;
302 }
303
304 return;
305 }
306 return;
307err:
308 regs->verdict.code = NFT_BREAK;
309}
310
311static void nft_exthdr_sctp_eval(const struct nft_expr *expr,
312 struct nft_regs *regs,
313 const struct nft_pktinfo *pkt)
314{
315 unsigned int offset = nft_thoff(pkt) + sizeof(struct sctphdr);
316 struct nft_exthdr *priv = nft_expr_priv(expr);
317 u32 *dest = ®s->data[priv->dreg];
318 const struct sctp_chunkhdr *sch;
319 struct sctp_chunkhdr _sch;
320
321 if (pkt->tprot != IPPROTO_SCTP)
322 goto err;
323
324 do {
325 sch = skb_header_pointer(pkt->skb, offset, sizeof(_sch), &_sch);
326 if (!sch || !sch->length)
327 break;
328
329 if (sch->type == priv->type) {
330 if (priv->flags & NFT_EXTHDR_F_PRESENT) {
331 nft_reg_store8(dest, true);
332 return;
333 }
334 if (priv->offset + priv->len > ntohs(sch->length) ||
335 offset + ntohs(sch->length) > pkt->skb->len)
336 break;
337
338 dest[priv->len / NFT_REG32_SIZE] = 0;
339 if (skb_copy_bits(pkt->skb, offset + priv->offset,
340 dest, priv->len) < 0)
341 break;
342 return;
343 }
344 offset += SCTP_PAD4(ntohs(sch->length));
345 } while (offset < pkt->skb->len);
346err:
347 if (priv->flags & NFT_EXTHDR_F_PRESENT)
348 nft_reg_store8(dest, false);
349 else
350 regs->verdict.code = NFT_BREAK;
351}
352
353static const struct nla_policy nft_exthdr_policy[NFTA_EXTHDR_MAX + 1] = {
354 [NFTA_EXTHDR_DREG] = { .type = NLA_U32 },
355 [NFTA_EXTHDR_TYPE] = { .type = NLA_U8 },
356 [NFTA_EXTHDR_OFFSET] = { .type = NLA_U32 },
357 [NFTA_EXTHDR_LEN] = { .type = NLA_U32 },
358 [NFTA_EXTHDR_FLAGS] = { .type = NLA_U32 },
359 [NFTA_EXTHDR_OP] = { .type = NLA_U32 },
360 [NFTA_EXTHDR_SREG] = { .type = NLA_U32 },
361};
362
363static int nft_exthdr_init(const struct nft_ctx *ctx,
364 const struct nft_expr *expr,
365 const struct nlattr * const tb[])
366{
367 struct nft_exthdr *priv = nft_expr_priv(expr);
368 u32 offset, len, flags = 0, op = NFT_EXTHDR_OP_IPV6;
369 int err;
370
371 if (!tb[NFTA_EXTHDR_DREG] ||
372 !tb[NFTA_EXTHDR_TYPE] ||
373 !tb[NFTA_EXTHDR_OFFSET] ||
374 !tb[NFTA_EXTHDR_LEN])
375 return -EINVAL;
376
377 err = nft_parse_u32_check(tb[NFTA_EXTHDR_OFFSET], U8_MAX, &offset);
378 if (err < 0)
379 return err;
380
381 err = nft_parse_u32_check(tb[NFTA_EXTHDR_LEN], U8_MAX, &len);
382 if (err < 0)
383 return err;
384
385 if (tb[NFTA_EXTHDR_FLAGS]) {
386 err = nft_parse_u32_check(tb[NFTA_EXTHDR_FLAGS], U8_MAX, &flags);
387 if (err < 0)
388 return err;
389
390 if (flags & ~NFT_EXTHDR_F_PRESENT)
391 return -EINVAL;
392 }
393
394 if (tb[NFTA_EXTHDR_OP]) {
395 err = nft_parse_u32_check(tb[NFTA_EXTHDR_OP], U8_MAX, &op);
396 if (err < 0)
397 return err;
398 }
399
400 priv->type = nla_get_u8(tb[NFTA_EXTHDR_TYPE]);
401 priv->offset = offset;
402 priv->len = len;
403 priv->flags = flags;
404 priv->op = op;
405
406 return nft_parse_register_store(ctx, tb[NFTA_EXTHDR_DREG],
407 &priv->dreg, NULL, NFT_DATA_VALUE,
408 priv->len);
409}
410
411static int nft_exthdr_tcp_set_init(const struct nft_ctx *ctx,
412 const struct nft_expr *expr,
413 const struct nlattr * const tb[])
414{
415 struct nft_exthdr *priv = nft_expr_priv(expr);
416 u32 offset, len, flags = 0, op = NFT_EXTHDR_OP_IPV6;
417 int err;
418
419 if (!tb[NFTA_EXTHDR_SREG] ||
420 !tb[NFTA_EXTHDR_TYPE] ||
421 !tb[NFTA_EXTHDR_OFFSET] ||
422 !tb[NFTA_EXTHDR_LEN])
423 return -EINVAL;
424
425 if (tb[NFTA_EXTHDR_DREG] || tb[NFTA_EXTHDR_FLAGS])
426 return -EINVAL;
427
428 err = nft_parse_u32_check(tb[NFTA_EXTHDR_OFFSET], U8_MAX, &offset);
429 if (err < 0)
430 return err;
431
432 err = nft_parse_u32_check(tb[NFTA_EXTHDR_LEN], U8_MAX, &len);
433 if (err < 0)
434 return err;
435
436 if (offset < 2)
437 return -EOPNOTSUPP;
438
439 switch (len) {
440 case 2: break;
441 case 4: break;
442 default:
443 return -EOPNOTSUPP;
444 }
445
446 err = nft_parse_u32_check(tb[NFTA_EXTHDR_OP], U8_MAX, &op);
447 if (err < 0)
448 return err;
449
450 priv->type = nla_get_u8(tb[NFTA_EXTHDR_TYPE]);
451 priv->offset = offset;
452 priv->len = len;
453 priv->flags = flags;
454 priv->op = op;
455
456 return nft_parse_register_load(tb[NFTA_EXTHDR_SREG], &priv->sreg,
457 priv->len);
458}
459
460static int nft_exthdr_ipv4_init(const struct nft_ctx *ctx,
461 const struct nft_expr *expr,
462 const struct nlattr * const tb[])
463{
464 struct nft_exthdr *priv = nft_expr_priv(expr);
465 int err = nft_exthdr_init(ctx, expr, tb);
466
467 if (err < 0)
468 return err;
469
470 switch (priv->type) {
471 case IPOPT_SSRR:
472 case IPOPT_LSRR:
473 case IPOPT_RR:
474 case IPOPT_RA:
475 break;
476 default:
477 return -EOPNOTSUPP;
478 }
479 return 0;
480}
481
482static int nft_exthdr_dump_common(struct sk_buff *skb, const struct nft_exthdr *priv)
483{
484 if (nla_put_u8(skb, NFTA_EXTHDR_TYPE, priv->type))
485 goto nla_put_failure;
486 if (nla_put_be32(skb, NFTA_EXTHDR_OFFSET, htonl(priv->offset)))
487 goto nla_put_failure;
488 if (nla_put_be32(skb, NFTA_EXTHDR_LEN, htonl(priv->len)))
489 goto nla_put_failure;
490 if (nla_put_be32(skb, NFTA_EXTHDR_FLAGS, htonl(priv->flags)))
491 goto nla_put_failure;
492 if (nla_put_be32(skb, NFTA_EXTHDR_OP, htonl(priv->op)))
493 goto nla_put_failure;
494 return 0;
495
496nla_put_failure:
497 return -1;
498}
499
500static int nft_exthdr_dump(struct sk_buff *skb, const struct nft_expr *expr)
501{
502 const struct nft_exthdr *priv = nft_expr_priv(expr);
503
504 if (nft_dump_register(skb, NFTA_EXTHDR_DREG, priv->dreg))
505 return -1;
506
507 return nft_exthdr_dump_common(skb, priv);
508}
509
510static int nft_exthdr_dump_set(struct sk_buff *skb, const struct nft_expr *expr)
511{
512 const struct nft_exthdr *priv = nft_expr_priv(expr);
513
514 if (nft_dump_register(skb, NFTA_EXTHDR_SREG, priv->sreg))
515 return -1;
516
517 return nft_exthdr_dump_common(skb, priv);
518}
519
520static const struct nft_expr_ops nft_exthdr_ipv6_ops = {
521 .type = &nft_exthdr_type,
522 .size = NFT_EXPR_SIZE(sizeof(struct nft_exthdr)),
523 .eval = nft_exthdr_ipv6_eval,
524 .init = nft_exthdr_init,
525 .dump = nft_exthdr_dump,
526};
527
528static const struct nft_expr_ops nft_exthdr_ipv4_ops = {
529 .type = &nft_exthdr_type,
530 .size = NFT_EXPR_SIZE(sizeof(struct nft_exthdr)),
531 .eval = nft_exthdr_ipv4_eval,
532 .init = nft_exthdr_ipv4_init,
533 .dump = nft_exthdr_dump,
534};
535
536static const struct nft_expr_ops nft_exthdr_tcp_ops = {
537 .type = &nft_exthdr_type,
538 .size = NFT_EXPR_SIZE(sizeof(struct nft_exthdr)),
539 .eval = nft_exthdr_tcp_eval,
540 .init = nft_exthdr_init,
541 .dump = nft_exthdr_dump,
542};
543
544static const struct nft_expr_ops nft_exthdr_tcp_set_ops = {
545 .type = &nft_exthdr_type,
546 .size = NFT_EXPR_SIZE(sizeof(struct nft_exthdr)),
547 .eval = nft_exthdr_tcp_set_eval,
548 .init = nft_exthdr_tcp_set_init,
549 .dump = nft_exthdr_dump_set,
550};
551
552static const struct nft_expr_ops nft_exthdr_sctp_ops = {
553 .type = &nft_exthdr_type,
554 .size = NFT_EXPR_SIZE(sizeof(struct nft_exthdr)),
555 .eval = nft_exthdr_sctp_eval,
556 .init = nft_exthdr_init,
557 .dump = nft_exthdr_dump,
558};
559
560static const struct nft_expr_ops *
561nft_exthdr_select_ops(const struct nft_ctx *ctx,
562 const struct nlattr * const tb[])
563{
564 u32 op;
565
566 if (!tb[NFTA_EXTHDR_OP])
567 return &nft_exthdr_ipv6_ops;
568
569 if (tb[NFTA_EXTHDR_SREG] && tb[NFTA_EXTHDR_DREG])
570 return ERR_PTR(-EOPNOTSUPP);
571
572 op = ntohl(nla_get_be32(tb[NFTA_EXTHDR_OP]));
573 switch (op) {
574 case NFT_EXTHDR_OP_TCPOPT:
575 if (tb[NFTA_EXTHDR_SREG])
576 return &nft_exthdr_tcp_set_ops;
577 if (tb[NFTA_EXTHDR_DREG])
578 return &nft_exthdr_tcp_ops;
579 break;
580 case NFT_EXTHDR_OP_IPV6:
581 if (tb[NFTA_EXTHDR_DREG])
582 return &nft_exthdr_ipv6_ops;
583 break;
584 case NFT_EXTHDR_OP_IPV4:
585 if (ctx->family != NFPROTO_IPV6) {
586 if (tb[NFTA_EXTHDR_DREG])
587 return &nft_exthdr_ipv4_ops;
588 }
589 break;
590 case NFT_EXTHDR_OP_SCTP:
591 if (tb[NFTA_EXTHDR_DREG])
592 return &nft_exthdr_sctp_ops;
593 break;
594 }
595
596 return ERR_PTR(-EOPNOTSUPP);
597}
598
599struct nft_expr_type nft_exthdr_type __read_mostly = {
600 .name = "exthdr",
601 .select_ops = nft_exthdr_select_ops,
602 .policy = nft_exthdr_policy,
603 .maxattr = NFTA_EXTHDR_MAX,
604 .owner = THIS_MODULE,
605};
606