[PATCH] extensions: masquerade: Add RFC-7597 section 5.1 PSID support

[Cole Dishington <Cole.Dishington@alliedtelesis.co.nz>]

Added --psid option to masquerade extension to specify port ranges, as
described in RFC-7597 section 5.1. The PSID option needs the base field
in range2, so add version 1 of the masquerade extension.

Signed-off-by: Cole Dishington <Cole.Dishington@alliedtelesis.co.nz>
---

Notes:
    Thanks for your time reviewing.
    
    Changes in v2:
    - Added test cases for MASQUERADE psid option.
    - Modified MASQUERADE_help_v1() to use print from MASQUERADE_help_v0.
    - Fixed discarded-qualifiers compiler warning.
    - Fixed _log2 infinite loop issue.
    - Improved error checking of psid option.
    - Added psid option to man page.

 extensions/libipt_MASQUERADE.c   | 285 +++++++++++++++++++++++++------
 extensions/libipt_MASQUERADE.t   |   7 +
 extensions/libxt_MASQUERADE.man  |   6 +-
 include/linux/netfilter/nf_nat.h |   5 +-
 4 files changed, 248 insertions(+), 55 deletions(-)

diff --git a/extensions/libipt_MASQUERADE.c b/extensions/libipt_MASQUERADE.c
index 90bf6065..46fbdc8a 100644
--- a/extensions/libipt_MASQUERADE.c
+++ b/extensions/libipt_MASQUERADE.c
@@ -12,9 +12,41 @@ enum {
 	O_TO_PORTS = 0,
 	O_RANDOM,
 	O_RANDOM_FULLY,
+	O_PSID,
 };
 
-static void MASQUERADE_help(void)
+static unsigned int _log2(unsigned int x)
+{
+	unsigned int y = 0;
+
+	for (; x > 1; x >>= 1)
+		y++;
+	return y;
+}
+
+static void cpy_ipv4_range_to_range2(struct nf_nat_range2 *dst, const struct nf_nat_ipv4_range *src)
+{
+	memset(&dst->min_addr, 0, sizeof(dst->min_addr));
+	memset(&dst->max_addr, 0, sizeof(dst->max_addr));
+	memset(&dst->base_proto, 0, sizeof(dst->base_proto));
+
+	dst->flags	     = src->flags;
+	dst->min_addr.ip = src->min_ip;
+	dst->max_addr.ip = src->max_ip;
+	dst->min_proto	 = src->min;
+	dst->max_proto	 = src->max;
+}
+
+static void cpy_range2_to_ipv4_range(struct nf_nat_ipv4_range *dst, const struct nf_nat_range2 *src)
+{
+	dst->flags	 = src->flags;
+	dst->min_ip  = src->min_addr.ip;
+	dst->max_ip  = src->max_addr.ip;
+	dst->min	 = src->min_proto;
+	dst->max	 = src->max_proto;
+}
+
+static void MASQUERADE_help_v0(void)
 {
 	printf(
 "MASQUERADE target options:\n"
@@ -26,14 +58,30 @@ static void MASQUERADE_help(void)
 "				Fully randomize source port.\n");
 }
 
-static const struct xt_option_entry MASQUERADE_opts[] = {
+static void MASQUERADE_help_v1(void)
+{
+	MASQUERADE_help_v0();
+	printf(
+" --psid <offset>:<psid>:<psid_length>\n"
+"				Run in PSID mode with this PSID\n");
+}
+
+static const struct xt_option_entry MASQUERADE_opts_v0[] = {
+	{.name = "to-ports", .id = O_TO_PORTS, .type = XTTYPE_STRING},
+	{.name = "random", .id = O_RANDOM, .type = XTTYPE_NONE},
+	{.name = "random-fully", .id = O_RANDOM_FULLY, .type = XTTYPE_NONE},
+	XTOPT_TABLEEND,
+};
+
+static const struct xt_option_entry MASQUERADE_opts_v1[] = {
 	{.name = "to-ports", .id = O_TO_PORTS, .type = XTTYPE_STRING},
 	{.name = "random", .id = O_RANDOM, .type = XTTYPE_NONE},
 	{.name = "random-fully", .id = O_RANDOM_FULLY, .type = XTTYPE_NONE},
+	{.name = "psid", .id = O_PSID, .type = XTTYPE_STRING},
 	XTOPT_TABLEEND,
 };
 
-static void MASQUERADE_init(struct xt_entry_target *t)
+static void MASQUERADE_init_v0(struct xt_entry_target *t)
 {
 	struct nf_nat_ipv4_multi_range_compat *mr = (struct nf_nat_ipv4_multi_range_compat *)t->data;
 
@@ -42,21 +90,20 @@ static void MASQUERADE_init(struct xt_entry_target *t)
 }
 
 /* Parses ports */
-static void
-parse_ports(const char *arg, struct nf_nat_ipv4_multi_range_compat *mr)
+static void parse_ports(const char *arg, struct nf_nat_range2 *r)
 {
 	char *end;
 	unsigned int port, maxport;
 
-	mr->range[0].flags |= NF_NAT_RANGE_PROTO_SPECIFIED;
+	r->flags |= NF_NAT_RANGE_PROTO_SPECIFIED;
 
 	if (!xtables_strtoui(arg, &end, &port, 0, UINT16_MAX))
 		xtables_param_act(XTF_BAD_VALUE, "MASQUERADE", "--to-ports", arg);
 
 	switch (*end) {
 	case '\0':
-		mr->range[0].min.tcp.port
-			= mr->range[0].max.tcp.port
+		r->min_proto.tcp.port
+			= r->max_proto.tcp.port
 			= htons(port);
 		return;
 	case '-':
@@ -66,8 +113,8 @@ parse_ports(const char *arg, struct nf_nat_ipv4_multi_range_compat *mr)
 		if (maxport < port)
 			break;
 
-		mr->range[0].min.tcp.port = htons(port);
-		mr->range[0].max.tcp.port = htons(maxport);
+		r->min_proto.tcp.port = htons(port);
+		r->max_proto.tcp.port = htons(maxport);
 		return;
 	default:
 		break;
@@ -75,11 +122,54 @@ parse_ports(const char *arg, struct nf_nat_ipv4_multi_range_compat *mr)
 	xtables_param_act(XTF_BAD_VALUE, "MASQUERADE", "--to-ports", arg);
 }
 
-static void MASQUERADE_parse(struct xt_option_call *cb)
+static void range_to_psid_args(const struct nf_nat_range2 *r, unsigned int *offset,
+			       unsigned int *psid, unsigned int *psid_length)
+{
+	unsigned int min, power_j;
+
+	min = htons(r->min_proto.all);
+	power_j = htons(r->max_proto.all) - min + 1;
+	*offset = ntohs(r->base_proto.all);
+	*psid = (min - *offset) >> _log2(power_j);
+	*psid_length = _log2(*offset/power_j);
+}
+
+static void parse_psid(const char *arg, struct nf_nat_range2 *r)
+{
+	char *end;
+	unsigned int offset, psid, psid_len;
+
+	if (!xtables_strtoui(arg, &end, &offset, 0, UINT16_MAX) || *end != ':' ||
+	    offset >= (1 << 16) || (1 << _log2(offset)) != offset)
+		xtables_param_act(XTF_BAD_VALUE, "MASQUERADE", "--psid <offset> invalid", arg);
+
+	if (!xtables_strtoui(end + 1, &end, &psid, 0, UINT16_MAX) || *end != ':')
+		xtables_param_act(XTF_BAD_VALUE, "MASQUERADE", "--psid <psid> invalid", arg);
+
+	if (!xtables_strtoui(end + 1, &end, &psid_len, 0, UINT16_MAX) || *end != '\0' ||
+	    psid_len >= 16)
+		xtables_param_act(XTF_BAD_VALUE, "MASQUERADE", "--psid <psid_length> invalid", arg);
+
+	if (psid >= (1 << psid_len))
+		xtables_param_act(XTF_BAD_VALUE, "MASQUERADE",
+				  "--psid <psid> too large for <psid_length>", arg);
+
+	if (psid_len + 16 - _log2(offset) >= 16)
+		xtables_param_act(XTF_BAD_VALUE, "MASQUERADE",
+				  "--psid <offset> and/or <psid_length> are too large", arg);
+
+	psid = psid << (_log2(offset/(1 << psid_len)));
+	r->min_proto.all = htons(offset + psid);
+	r->max_proto.all = htons(offset + psid + ((offset/(1 << psid_len)) - 1));
+	r->base_proto.all = htons(offset);
+	r->flags |= NF_NAT_RANGE_PSID;
+	r->flags |= NF_NAT_RANGE_PROTO_SPECIFIED;
+}
+
+static void _MASQUERADE_parse(struct xt_option_call *cb, struct nf_nat_range2 *r, int rev)
 {
 	const struct ipt_entry *entry = cb->xt_entry;
 	int portok;
-	struct nf_nat_ipv4_multi_range_compat *mr = cb->data;
 
 	if (entry->ip.proto == IPPROTO_TCP
 	    || entry->ip.proto == IPPROTO_UDP
@@ -96,29 +186,50 @@ static void MASQUERADE_parse(struct xt_option_call *cb)
 		if (!portok)
 			xtables_error(PARAMETER_PROBLEM,
 				   "Need TCP, UDP, SCTP or DCCP with port specification");
-		parse_ports(cb->arg, mr);
+		parse_ports(cb->arg, r);
 		break;
 	case O_RANDOM:
-		mr->range[0].flags |=  NF_NAT_RANGE_PROTO_RANDOM;
+		r->flags |=  NF_NAT_RANGE_PROTO_RANDOM;
 		break;
 	case O_RANDOM_FULLY:
-		mr->range[0].flags |=  NF_NAT_RANGE_PROTO_RANDOM_FULLY;
+		r->flags |=  NF_NAT_RANGE_PROTO_RANDOM_FULLY;
+		break;
+	case O_PSID:
+		parse_psid(cb->arg, r);
 		break;
 	}
 }
 
-static void
-MASQUERADE_print(const void *ip, const struct xt_entry_target *target,
-                 int numeric)
+static void MASQUERADE_parse_v0(struct xt_option_call *cb)
+{
+	struct nf_nat_ipv4_multi_range_compat *mr = (void *)cb->data;
+	struct nf_nat_range2 r = {};
+
+	cpy_ipv4_range_to_range2(&r, &mr->range[0]);
+	_MASQUERADE_parse(cb, &r, 0);
+	cpy_range2_to_ipv4_range(&mr->range[0], &r);
+}
+
+static void MASQUERADE_parse_v1(struct xt_option_call *cb)
+{
+	_MASQUERADE_parse(cb, (struct nf_nat_range2 *)cb->data, 1);
+}
+
+static void _MASQUERADE_print(const struct nf_nat_range2 *r, int rev)
 {
-	const struct nf_nat_ipv4_multi_range_compat *mr = (const void *)target->data;
-	const struct nf_nat_ipv4_range *r = &mr->range[0];
 
 	if (r->flags & NF_NAT_RANGE_PROTO_SPECIFIED) {
-		printf(" masq ports: ");
-		printf("%hu", ntohs(r->min.tcp.port));
-		if (r->max.tcp.port != r->min.tcp.port)
-			printf("-%hu", ntohs(r->max.tcp.port));
+		if (r->flags & NF_NAT_RANGE_PSID) {
+			unsigned int offset, psid, psid_length;
+
+			range_to_psid_args(r, &offset, &psid, &psid_length);
+			printf(" masq psid: %hu:%hu:%hu", offset, psid, psid_length);
+		} else {
+			printf(" masq ports: ");
+			printf("%hu", ntohs(r->min_proto.tcp.port));
+			if (r->max_proto.tcp.port != r->min_proto.tcp.port)
+				printf("-%hu", ntohs(r->max_proto.tcp.port));
+		}
 	}
 
 	if (r->flags & NF_NAT_RANGE_PROTO_RANDOM)
@@ -126,18 +237,37 @@ MASQUERADE_print(const void *ip, const struct xt_entry_target *target,
 
 	if (r->flags & NF_NAT_RANGE_PROTO_RANDOM_FULLY)
 		printf(" random-fully");
+
+
 }
 
-static void
-MASQUERADE_save(const void *ip, const struct xt_entry_target *target)
+static void MASQUERADE_print_v0(const void *ip, const struct xt_entry_target *target, int numeric)
 {
 	const struct nf_nat_ipv4_multi_range_compat *mr = (const void *)target->data;
-	const struct nf_nat_ipv4_range *r = &mr->range[0];
+	struct nf_nat_range2 r = {};
 
+	cpy_ipv4_range_to_range2(&r, &mr->range[0]);
+	_MASQUERADE_print(&r, 0);
+}
+
+static void MASQUERADE_print_v1(const void *ip, const struct xt_entry_target *target, int numeric)
+{
+	_MASQUERADE_print((const struct nf_nat_range2 *)target->data, 1);
+}
+
+static void _MASQUERADE_save(const struct nf_nat_range2 *r, int rev)
+{
 	if (r->flags & NF_NAT_RANGE_PROTO_SPECIFIED) {
-		printf(" --to-ports %hu", ntohs(r->min.tcp.port));
-		if (r->max.tcp.port != r->min.tcp.port)
-			printf("-%hu", ntohs(r->max.tcp.port));
+		if (r->flags & NF_NAT_RANGE_PSID) {
+			unsigned int offset, psid, psid_length;
+
+			range_to_psid_args(r, &offset, &psid, &psid_length);
+			printf(" --psid %hu:%hu:%hu", offset, psid, psid_length);
+		} else {
+			printf(" --to-ports %hu", ntohs(r->min_proto.tcp.port));
+			if (r->max_proto.tcp.port != r->min_proto.tcp.port)
+				printf("-%hu", ntohs(r->max_proto.tcp.port));
+		}
 	}
 
 	if (r->flags & NF_NAT_RANGE_PROTO_RANDOM)
@@ -147,44 +277,93 @@ MASQUERADE_save(const void *ip, const struct xt_entry_target *target)
 		printf(" --random-fully");
 }
 
-static int MASQUERADE_xlate(struct xt_xlate *xl,
-			    const struct xt_xlate_tg_params *params)
+static void MASQUERADE_save_v0(const void *ip, const struct xt_entry_target *target)
 {
-	const struct nf_nat_ipv4_multi_range_compat *mr =
-		(const void *)params->target->data;
-	const struct nf_nat_ipv4_range *r = &mr->range[0];
+	const struct nf_nat_ipv4_multi_range_compat *mr = (const void *)target->data;
+	struct nf_nat_range2 r = {};
 
+	cpy_ipv4_range_to_range2(&r, &mr->range[0]);
+	_MASQUERADE_save(&r, 0);
+}
+
+static void MASQUERADE_save_v1(const void *ip, const struct xt_entry_target *target)
+{
+	_MASQUERADE_save((const struct nf_nat_range2 *)target->data, 1);
+}
+
+static void _MASQUERADE_xlate(struct xt_xlate *xl, const struct nf_nat_range2 *r, int rev)
+{
 	xt_xlate_add(xl, "masquerade");
 
 	if (r->flags & NF_NAT_RANGE_PROTO_SPECIFIED) {
-		xt_xlate_add(xl, " to :%hu", ntohs(r->min.tcp.port));
-		if (r->max.tcp.port != r->min.tcp.port)
-			xt_xlate_add(xl, "-%hu", ntohs(r->max.tcp.port));
-        }
+		if (r->flags & NF_NAT_RANGE_PSID) {
+			unsigned int offset, psid, psid_length;
+
+			range_to_psid_args(r, &offset, &psid, &psid_length);
+			xt_xlate_add(xl, " psid %hu:%hu:%hu", offset, psid, psid_length);
+		} else {
+			xt_xlate_add(xl, " to :%hu", ntohs(r->min_proto.tcp.port));
+			if (r->max_proto.tcp.port != r->min_proto.tcp.port)
+				xt_xlate_add(xl, "-%hu", ntohs(r->max_proto.tcp.port));
+		}
+	}
 
 	xt_xlate_add(xl, " ");
 	if (r->flags & NF_NAT_RANGE_PROTO_RANDOM)
 		xt_xlate_add(xl, "random ");
+}
 
+static int MASQUERADE_xlate_v0(struct xt_xlate *xl, const struct xt_xlate_tg_params *params)
+{
+	const struct nf_nat_ipv4_multi_range_compat *mr =
+		(const void *)params->target->data;
+	struct nf_nat_range2 r = {};
+
+	cpy_ipv4_range_to_range2(&r, &mr->range[0]);
+	_MASQUERADE_xlate(xl, &r, 0);
+
+	return 1;
+}
+
+static int MASQUERADE_xlate_v1(struct xt_xlate *xl, const struct xt_xlate_tg_params *params)
+{
+	_MASQUERADE_xlate(xl, (const struct nf_nat_range2 *)params->target->data, 1);
 	return 1;
 }
 
-static struct xtables_target masquerade_tg_reg = {
-	.name		= "MASQUERADE",
-	.version	= XTABLES_VERSION,
-	.family		= NFPROTO_IPV4,
-	.size		= XT_ALIGN(sizeof(struct nf_nat_ipv4_multi_range_compat)),
-	.userspacesize	= XT_ALIGN(sizeof(struct nf_nat_ipv4_multi_range_compat)),
-	.help		= MASQUERADE_help,
-	.init		= MASQUERADE_init,
-	.x6_parse	= MASQUERADE_parse,
-	.print		= MASQUERADE_print,
-	.save		= MASQUERADE_save,
-	.x6_options	= MASQUERADE_opts,
-	.xlate		= MASQUERADE_xlate,
+static struct xtables_target masquerade_tg_reg[] = {
+	{
+		.name		= "MASQUERADE",
+		.version	= XTABLES_VERSION,
+		.family		= NFPROTO_IPV4,
+		.revision	= 0,
+		.size		= XT_ALIGN(sizeof(struct nf_nat_ipv4_multi_range_compat)),
+		.userspacesize	= XT_ALIGN(sizeof(struct nf_nat_ipv4_multi_range_compat)),
+		.help		= MASQUERADE_help_v0,
+		.init		= MASQUERADE_init_v0,
+		.x6_parse	= MASQUERADE_parse_v0,
+		.print		= MASQUERADE_print_v0,
+		.save		= MASQUERADE_save_v0,
+		.x6_options	= MASQUERADE_opts_v0,
+		.xlate		= MASQUERADE_xlate_v0,
+	},
+	{
+		.name		= "MASQUERADE",
+		.version	= XTABLES_VERSION,
+		.family		= NFPROTO_IPV4,
+		.revision	= 1,
+		.size		= XT_ALIGN(sizeof(struct nf_nat_range2)),
+		.userspacesize	= XT_ALIGN(sizeof(struct nf_nat_range2)),
+		.help		= MASQUERADE_help_v1,
+		.x6_parse	= MASQUERADE_parse_v1,
+		.print		= MASQUERADE_print_v1,
+		.save		= MASQUERADE_save_v1,
+		.x6_options	= MASQUERADE_opts_v1,
+		.xlate		= MASQUERADE_xlate_v1,
+	},
 };
 
 void _init(void)
 {
-	xtables_register_target(&masquerade_tg_reg);
+	xtables_register_targets(masquerade_tg_reg, ARRAY_SIZE(masquerade_tg_reg));
 }
diff --git a/extensions/libipt_MASQUERADE.t b/extensions/libipt_MASQUERADE.t
index e25d2a04..b69ea97e 100644
--- a/extensions/libipt_MASQUERADE.t
+++ b/extensions/libipt_MASQUERADE.t
@@ -7,3 +7,10 @@
 -p udp -j MASQUERADE --to-ports 1024-65535;=;OK
 -p udp -j MASQUERADE --to-ports 1024-65536;;FAIL
 -p udp -j MASQUERADE --to-ports -1;;FAIL
+-j MASQUERADE --psid 1024;;FAIL
+-j MASQUERADE --psid 1024:0;;FAIL
+-j MASQUERADE --psid 1024:0:8;=;OK
+-j MASQUERADE --psid -1024:-52:-8;=;FAIL
+-j MASQUERADE --psid 1024:52:8;=;OK
+-j MASQUERADE --psid 1024:270:8;;FAIL
+-j MASQUERADE --psid 1024:270:8;;FAIL
diff --git a/extensions/libxt_MASQUERADE.man b/extensions/libxt_MASQUERADE.man
index 7746f473..f6fb528c 100644
--- a/extensions/libxt_MASQUERADE.man
+++ b/extensions/libxt_MASQUERADE.man
@@ -32,4 +32,8 @@ If option
 \fB\-\-random-fully\fP
 is used then port mapping will be fully randomized (kernel >= 3.13).
 .TP
-IPv6 support available since Linux kernels >= 3.7.
+\fB\-\-psid\fP \fIoffset\fB:\fIpsid\fB:\fIpsid_length
+This specifies a range of source ports to use based on RFC-7597 PSID (kernel >= 5.13), overriding the source ports.
+  \fIoffset\fP : Excluded ports (0 to \fIoffset\fP - 1) and the gap between port ranges for a given \fIpsid\fP.
+  \fIpsid\fP   : Port ranges used by this rule.
+  \fIoffset\fP : Bit-length of the psid field.
diff --git a/include/linux/netfilter/nf_nat.h b/include/linux/netfilter/nf_nat.h
index b600000d..0f004765 100644
--- a/include/linux/netfilter/nf_nat.h
+++ b/include/linux/netfilter/nf_nat.h
@@ -10,6 +10,8 @@
 #define NF_NAT_RANGE_PERSISTENT			(1 << 3)
 #define NF_NAT_RANGE_PROTO_RANDOM_FULLY		(1 << 4)
 #define NF_NAT_RANGE_PROTO_OFFSET		(1 << 5)
+#define NF_NAT_RANGE_NETMAP			(1 << 6)
+#define NF_NAT_RANGE_PSID			(1 << 7)
 
 #define NF_NAT_RANGE_PROTO_RANDOM_ALL		\
 	(NF_NAT_RANGE_PROTO_RANDOM | NF_NAT_RANGE_PROTO_RANDOM_FULLY)
@@ -17,7 +19,8 @@
 #define NF_NAT_RANGE_MASK					\
 	(NF_NAT_RANGE_MAP_IPS | NF_NAT_RANGE_PROTO_SPECIFIED |	\
 	 NF_NAT_RANGE_PROTO_RANDOM | NF_NAT_RANGE_PERSISTENT |	\
-	 NF_NAT_RANGE_PROTO_RANDOM_FULLY | NF_NAT_RANGE_PROTO_OFFSET)
+	 NF_NAT_RANGE_PROTO_RANDOM_FULLY | NF_NAT_RANGE_PROTO_OFFSET | \
+	 NF_NAT_RANGE_NETMAP | NF_NAT_RANGE_PSID)
 
 struct nf_nat_ipv4_range {
 	unsigned int			flags;
-- 
2.32.0