[PATCH] netfilter: xt_esp: add support for ESP match in NAT Traversal

[Wei Han <lailitty@foxmail.com>]

when the ESP packets traversing Network Address Translators,
which are encapsulated and decapsulated inside UDP packets,
so we need to get ESP data in UDP.

Signed-off-by: Wei Han <lailitty@foxmail.com>
---
 net/netfilter/xt_esp.c | 54 +++++++++++++++++++++++++++++++++++-------
 1 file changed, 45 insertions(+), 9 deletions(-)

diff --git a/net/netfilter/xt_esp.c b/net/netfilter/xt_esp.c
index 2a1c0ad0ff07..c3feb79a830a 100644
--- a/net/netfilter/xt_esp.c
+++ b/net/netfilter/xt_esp.c
@@ -8,12 +8,14 @@
 #include <linux/skbuff.h>
 #include <linux/in.h>
 #include <linux/ip.h>
+#include <linux/ipv6.h>
 
 #include <linux/netfilter/xt_esp.h>
 #include <linux/netfilter/x_tables.h>
 
 #include <linux/netfilter_ipv4/ip_tables.h>
 #include <linux/netfilter_ipv6/ip6_tables.h>
+#include <net/ip.h>
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Yon Uriarte <yon@astaro.de>");
@@ -39,17 +41,53 @@ static bool esp_mt(const struct sk_buff *skb, struct xt_action_param *par)
 	struct ip_esp_hdr _esp;
 	const struct xt_esp *espinfo = par->matchinfo;
 
+	const struct iphdr *iph = NULL;
+	const struct ipv6hdr *ip6h = NULL;
+	const struct udphdr *udph = NULL;
+	struct udphdr _udph;
+	int proto = -1;
+
 	/* Must not be a fragment. */
 	if (par->fragoff != 0)
 		return false;
 
-	eh = skb_header_pointer(skb, par->thoff, sizeof(_esp), &_esp);
-	if (eh == NULL) {
-		/* We've been asked to examine this packet, and we
-		 * can't.  Hence, no choice but to drop.
-		 */
-		pr_debug("Dropping evil ESP tinygram.\n");
-		par->hotdrop = true;
+	if (xt_family(par) == NFPROTO_IPV6) {
+		ip6h = ipv6_hdr(skb);
+		if (!ip6h)
+			return false;
+		proto = ip6h->nexthdr;
+	} else {
+		iph = ip_hdr(skb);
+		if (!iph)
+			return false;
+		proto = iph->protocol;
+	}
+
+	if (proto == IPPROTO_UDP) {
+		//for NAT-T
+		udph = skb_header_pointer(skb, par->thoff, sizeof(_udph), &_udph);
+		if (udph && (udph->source == htons(4500) || udph->dest == htons(4500))) {
+			/* Not deal with above data it don't conflict with SPI
+			 * 1.IKE Header Format for Port 4500(Non-ESP Marker 0x00000000)
+			 * 2.NAT-Keepalive Packet Format(0xFF)
+			 */
+			eh = (struct ip_esp_hdr *)((char *)udph + sizeof(struct udphdr));
+		} else {
+			return false;
+		}
+	} else if (proto == IPPROTO_ESP) {
+		//not NAT-T
+		eh = skb_header_pointer(skb, par->thoff, sizeof(_esp), &_esp);
+		if (!eh) {
+			/* We've been asked to examine this packet, and we
+			 * can't.  Hence, no choice but to drop.
+			 */
+			pr_debug("Dropping evil ESP tinygram.\n");
+			par->hotdrop = true;
+			return false;
+		}
+	} else {
+		//not esp data
 		return false;
 	}
 
@@ -76,7 +114,6 @@ static struct xt_match esp_mt_reg[] __read_mostly = {
 		.checkentry	= esp_mt_check,
 		.match		= esp_mt,
 		.matchsize	= sizeof(struct xt_esp),
-		.proto		= IPPROTO_ESP,
 		.me		= THIS_MODULE,
 	},
 	{
@@ -85,7 +122,6 @@ static struct xt_match esp_mt_reg[] __read_mostly = {
 		.checkentry	= esp_mt_check,
 		.match		= esp_mt,
 		.matchsize	= sizeof(struct xt_esp),
-		.proto		= IPPROTO_ESP,
 		.me		= THIS_MODULE,
 	},
 };
-- 
2.17.1