Add xt_PROTO extension

Signed-off-by: Aron Xu <happyaron.xu@gmail.com>

extensions/libxt_PROTO.c

@@ -0,0 +1,113 @@
+/*
+ * PROTO Target module
+ * This program is distributed under the terms of GNU GPL
+ */
+#include <stdio.h>
+#include <xtables.h>
+#include "xt_PROTO.h"
+
+enum {
+	O_PROTO_SET = 0,
+	O_PROTO_STOP_AT_FRAG = 1,
+	O_PROTO_STOP_AT_AUTH = 2,
+	F_PROTO_SET = 1 << O_PROTO_SET,
+	F_PROTO_STOP_AT_FRAG = 1 << O_PROTO_STOP_AT_FRAG,
+	F_PROTO_STOP_AT_AUTH = 1 << O_PROTO_STOP_AT_AUTH,
+};
+
+#define s struct xt_PROTO_info
+static const struct xt_option_entry PROTO_opts[] = {
+	{.name = "proto-set", .type = XTTYPE_UINT8, .id = O_PROTO_SET,
+	 .flags = XTOPT_PUT | XTOPT_MAND, XTOPT_POINTER(s, proto)},
+	{.name = "stop-at-frag", .type = XTTYPE_NONE, .id = O_PROTO_STOP_AT_FRAG},
+	{.name = "stop-at-auth", .type = XTTYPE_NONE, .id = O_PROTO_STOP_AT_AUTH},
+	XTOPT_TABLEEND,
+};
+#undef s
+
+static void PROTO_help(void)
+{
+	printf(
+"PROTO target options\n"
+"  --proto-set value		Set protocol to <value 0-255>\n"
+	);
+}
+
+static void PROTO_parse(struct xt_option_call *cb)
+{
+	struct xt_PROTO_info *info = cb->data;
+
+	xtables_option_parse(cb);
+	switch (cb->entry->id) {
+	case O_PROTO_SET:
+		info->mode |= 1 << XT_PROTO_SET;
+		break;
+	case O_PROTO_STOP_AT_FRAG:
+		info->mode |= 1 << XT_PROTO_STOP_AT_FRAG;
+		break;
+	case O_PROTO_STOP_AT_AUTH:
+		info->mode |= 1 << XT_PROTO_STOP_AT_AUTH;
+		break;
+	}
+}
+
+static void PROTO_check(struct xt_fcheck_call *cb)
+{
+	if (!(cb->xflags & F_PROTO_SET))
+		xtables_error(PARAMETER_PROBLEM,
+				"PROTO: You must specify the proto to be set");
+}
+
+static void PROTO_save(const void *ip, const struct xt_entry_target *target)
+{
+	const struct xt_PROTO_info *info = 
+		(struct xt_PROTO_info *) target->data;
+
+	if(info->mode & (1 << XT_PROTO_SET)){
+		printf(" --proto-set %u", info->proto);
+	} 
+	if(info->mode & (1 << XT_PROTO_STOP_AT_FRAG)){
+		printf(" --stop-at-frag");
+	} 
+	if(info->mode & (1 << XT_PROTO_STOP_AT_AUTH)){
+		printf(" --stop-at-auth");
+	} 
+}
+
+static void PROTO_print(const void *ip, const struct xt_entry_target *target,
+                     int numeric)
+{
+	const struct xt_PROTO_info *info =
+		(struct xt_PROTO_info *) target->data;
+
+	printf(" PROTO ");
+	if(info->mode & (1 << XT_PROTO_SET)){
+		printf("set to %u", info->proto);
+	} 
+	if(info->mode & (1 << XT_PROTO_STOP_AT_FRAG)){
+		printf(" stop-at-frag");
+	} 
+	if(info->mode & (1 << XT_PROTO_STOP_AT_AUTH)){
+		printf(" stop-at-auth");
+	} 
+}
+
+static struct xtables_target proto_tg_reg = {
+	.name 		= "PROTO",
+	.version	= XTABLES_VERSION,
+	.family		= NFPROTO_UNSPEC,
+	.size		= XT_ALIGN(sizeof(struct xt_PROTO_info)),
+	.userspacesize	= XT_ALIGN(sizeof(struct xt_PROTO_info)),
+	.help		= PROTO_help,
+	.print		= PROTO_print,
+	.save		= PROTO_save,
+	.x6_parse	= PROTO_parse,
+	.x6_fcheck	= PROTO_check,
+	.x6_options	= PROTO_opts,
+};
+
+static __attribute__((constructor)) void _init(void)
+{
+	xtables_register_target(&proto_tg_reg);
+
+}

extensions/xt_PROTO.c

@@ -0,0 +1,172 @@
+/*
+ * Protocol modification target for IP tables
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+#include <linux/module.h>
+#include <linux/skbuff.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <net/ipv6.h>
+#include <net/checksum.h>
+
+
+#include <linux/netfilter/x_tables.h>
+#include "xt_PROTO.h"
+
+MODULE_AUTHOR("Shanker Wang <i@innull.com>");
+MODULE_DESCRIPTION("Xtables: Protocol field modification target");
+MODULE_LICENSE("GPL");
+
+static unsigned int
+proto_tg(struct sk_buff *skb, const struct xt_action_param *par)
+{
+	struct iphdr *iph;
+	const struct xt_PROTO_info *info = par->targinfo;
+	int new_proto;
+
+	if (!skb_make_writable(skb, skb->len))
+		return NF_DROP;
+
+	iph = ip_hdr(skb);
+
+	new_proto = iph->protocol;
+	if(info->mode & (1 << XT_PROTO_SET)){
+		new_proto = info->proto;
+	}
+	if (new_proto != iph->protocol) {
+		csum_replace2(&iph->check, htons(iph->protocol & 0xff),
+					   htons(new_proto & 0xff));
+		iph->protocol = new_proto;
+	}
+
+	return XT_CONTINUE;
+}
+
+static unsigned int
+proto_tg6(struct sk_buff *skb, const struct xt_action_param *par)
+{
+	struct ipv6hdr *ip6h;
+	const struct xt_PROTO_info *info = par->targinfo;
+	u8 *nexthdr; 
+	unsigned int hdr_offset;
+	__be16 *fp;
+
+	if (!skb_make_writable(skb, skb->len))
+		return NF_DROP;
+
+	ip6h = ipv6_hdr(skb);
+	nexthdr = &ip6h->nexthdr;
+
+	hdr_offset = sizeof(struct ipv6hdr);
+
+	for(;;){
+		struct ipv6_opt_hdr _opthdr, *opthp;
+		unsigned int hdrlen;
+		unsigned short _frag_off;
+		if ((!ipv6_ext_hdr(*nexthdr)) || *nexthdr == NEXTHDR_NONE) {
+			break;
+		}
+		opthp = skb_header_pointer(skb, skb_network_offset(skb) + hdr_offset, sizeof(_opthdr), &_opthdr);
+		if(!opthp){
+			return NF_DROP;
+		}
+		if(*nexthdr == NEXTHDR_FRAGMENT){
+			if(info->mode & (1 << XT_PROTO_STOP_AT_FRAG)){
+				break;
+			}
+			fp = skb_header_pointer(skb,
+						skb_network_offset(skb) + hdr_offset + 
+							offsetof(struct frag_hdr,
+							       frag_off),
+						sizeof(_frag_off),
+						&_frag_off);
+			if (!fp)
+				return NF_DROP;
+			_frag_off = ntohs(*fp) & ~0x7;
+			if(_frag_off){ // if the packet is not the first fragment
+				if ((!ipv6_ext_hdr(opthp->nexthdr)) || opthp->nexthdr == NEXTHDR_NONE || 
+					((info->mode & (1 << XT_PROTO_STOP_AT_AUTH)) && opthp->nexthdr == NEXTHDR_AUTH)
+				) {
+					nexthdr = &((struct ipv6_opt_hdr*)(skb_network_header(skb) + hdr_offset))->nexthdr;
+					break;
+				}else{
+					return XT_CONTINUE;
+				}
+			}
+			hdrlen = 8;
+		}else if(*nexthdr == NEXTHDR_AUTH){
+			if(info->mode & (1 << XT_PROTO_STOP_AT_AUTH)){
+				break;
+			}
+			hdrlen = (opthp->hdrlen + 2) << 2;
+		}else{
+			hdrlen = ipv6_optlen(opthp);
+		}
+		nexthdr = &((struct ipv6_opt_hdr*)(skb_network_header(skb) + hdr_offset))->nexthdr;
+		hdr_offset += hdrlen;
+	}
+	
+	if(info->mode & (1 << XT_PROTO_SET)){
+		*nexthdr = info->proto;
+	}
+
+	return XT_CONTINUE;
+}
+
+static int proto_tg_check(const struct xt_tgchk_param *par)
+{
+	const struct xt_PROTO_info *info = par->targinfo;
+
+	if ((info->mode & (1 << XT_PROTO_SET)) == 0){
+		pr_info_ratelimited("Did not specify any proto to set\n");
+		return -EINVAL;
+	}
+	if ((par->family != NFPROTO_IPV6) && ((info->mode & ((1 << XT_PROTO_STOP_AT_FRAG) | (1 << XT_PROTO_STOP_AT_AUTH))) != 0)){
+		pr_info_ratelimited("Must not specify stop-at-frag and stop-at-auth on non-ipv6 targets\n"); 
+		return -EPROTOTYPE;
+	}
+	return 0;
+}
+
+static struct xt_target proto_tg_reg[] __read_mostly = {
+	{
+		.name       = "PROTO",
+		.revision   = 0,
+		.family     = NFPROTO_IPV4,
+		.target     = proto_tg,
+		.targetsize = sizeof(struct xt_PROTO_info),
+		.table      = "mangle",
+		.checkentry = proto_tg_check,
+		.me         = THIS_MODULE,
+	},
+	{
+		.name       = "PROTO",
+		.revision   = 0,
+		.family     = NFPROTO_IPV6,
+		.target     = proto_tg6,
+		.targetsize = sizeof(struct xt_PROTO_info),
+		.table      = "mangle",
+		.checkentry = proto_tg_check,
+		.me         = THIS_MODULE,
+	},
+};
+
+static int __init proto_tg_init(void)
+{
+	return xt_register_targets(proto_tg_reg, ARRAY_SIZE(proto_tg_reg));
+}
+
+static void __exit proto_tg_exit(void)
+{
+	xt_unregister_targets(proto_tg_reg, ARRAY_SIZE(proto_tg_reg));
+}
+
+module_init(proto_tg_init);
+module_exit(proto_tg_exit);
+MODULE_ALIAS("ipt_PROTO");
+MODULE_ALIAS("ip6t_PROTO");
+

extensions/xt_PROTO.h

@@ -0,0 +1,20 @@
+/* SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note */
+/* Protocol modification module for IP tables */
+
+#ifndef _XT_PROTO_H
+#define _XT_PROTO_H
+
+#include <linux/types.h>
+
+enum {
+	XT_PROTO_SET = 0,
+	XT_PROTO_STOP_AT_FRAG = 1,
+	XT_PROTO_STOP_AT_AUTH = 2
+};
+
+struct xt_PROTO_info {
+	__u8	mode;
+	__u8	proto;
+};
+
+#endif