[PATCH net v3 2/4] psp: support ipv6 extension headers in psp_dev_rcv() decap

[Todd Freed <toddfreed@meta.com>]

psp_dev_rcv() expects the udp header to immediately follow the fixed
ipv6 header, so packets with ipv6 extension headers are dropped by the
is_udp check. additionally, on decap it writes the inner nexthdr into
the fixed header instead of into the last extension header.

fix by skipping the extension headers to find the udp header, and on
decap, setting the inner nexthdr on the last one.

fragment and authentication headers (ah) are not supported in psp
transport mode, so such packets are dropped.

Fixes: 0eddb8023cee ("psp: provide decapsulation and receive helper for drivers")
Signed-off-by: Todd Freed <toddfreed@meta.com>
---
 net/psp/psp_main.c | 62 +++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 59 insertions(+), 3 deletions(-)

diff --git a/net/psp/psp_main.c b/net/psp/psp_main.c
index c9c1a8826b7f..946047440ed3 100644
--- a/net/psp/psp_main.c
+++ b/net/psp/psp_main.c
@@ -4,6 +4,7 @@
 #include <linux/list.h>
 #include <linux/netdevice.h>
 #include <linux/xarray.h>
+#include <net/ipv6.h>
 #include <net/net_namespace.h>
 #include <net/psp.h>
 #include <net/udp.h>
@@ -185,6 +186,39 @@ unsigned int psp_key_size(u32 version)
 }
 EXPORT_SYMBOL(psp_key_size);
 
+/**
+ * psp_ipv6_nexthdr_off() - find the offset to the l4 proto nexthdr
+ * @hdr:	ipv6 fixed header
+ * @len:	l3 header total length
+ *
+ * Return: offset from @hdr, or a negative value to drop the packet
+ */
+static int psp_ipv6_nexthdr_off(const u8 *hdr, unsigned int len)
+{
+	unsigned int nh = offsetof(struct ipv6hdr, nexthdr);
+	unsigned int off = sizeof(struct ipv6hdr);
+	const struct ipv6_opt_hdr *eh;
+	unsigned int rem;
+	u8 type;
+
+	while (off < len) {
+		eh = (const void *)(hdr + off);
+		type = hdr[nh];
+
+		if (!ipv6_ext_hdr(type) || type == NEXTHDR_NONE ||
+		    type == NEXTHDR_FRAGMENT || type == NEXTHDR_AUTH)
+			return -EINVAL;
+		rem = len - off;
+		if (rem < sizeof(*eh))
+			return -EINVAL;
+
+		nh = off + offsetof(struct ipv6_opt_hdr, nexthdr);
+		off += ipv6_optlen(eh);
+	}
+
+	return off == len ? nh : -EINVAL;
+}
+
 static void psp_write_headers(struct net *net, struct sk_buff *skb, __be32 spi,
 			      u8 ver, unsigned int udp_len, __be16 sport)
 {
@@ -303,6 +337,7 @@ int psp_dev_rcv(struct sk_buff *skb, u16 dev_id, u8 generation, bool strip_icv)
 	struct psphdr *psph;
 	struct ethhdr *eth;
 	struct udphdr *uh;
+	int nh_off = 0;
 	__be16 proto;
 	bool is_udp;
 
@@ -331,8 +366,28 @@ int psp_dev_rcv(struct sk_buff *skb, u16 dev_id, u8 generation, bool strip_icv)
 			return -EINVAL;
 	} else {
 		struct ipv6hdr *ipv6h = (struct ipv6hdr *)(skb->data + l2_hlen);
+		u8 nexthdr = ipv6h->nexthdr;
+		__be16 frag_off;
+		int off;
+
+		/* UDP + PSP come after any extension headers */
+		off = ipv6_skip_exthdr(skb, l2_hlen + sizeof(*ipv6h), &nexthdr,
+				       &frag_off);
+		if (unlikely(off < 0))
+			return -EINVAL;
+
+		is_udp = nexthdr == IPPROTO_UDP;
+		l3_hlen = off - l2_hlen;
+		if (l3_hlen != sizeof(struct ipv6hdr) &&
+		    !pskb_may_pull(skb, l2_hlen + l3_hlen + PSP_ENCAP_HLEN))
+			return -EINVAL;
 
-		is_udp = ipv6h->nexthdr == IPPROTO_UDP;
+		/* fragment and authentication headers are not supported in
+		 * transport mode
+		 */
+		nh_off = psp_ipv6_nexthdr_off(skb->data + l2_hlen, l3_hlen);
+		if (unlikely(nh_off < 0))
+			return -EINVAL;
 	}
 
 	if (unlikely(!is_udp))
@@ -387,10 +442,11 @@ int psp_dev_rcv(struct sk_buff *skb, u16 dev_id, u8 generation, bool strip_icv)
 	} else {
 		struct ipv6hdr *ipv6h = (struct ipv6hdr *)(skb->data + l2_hlen);
 
-		if (unlikely(ntohs(ipv6h->payload_len) < encap))
+		if (unlikely(ntohs(ipv6h->payload_len) <
+			     l3_hlen - sizeof(*ipv6h) + encap))
 			return -EINVAL;
 
-		ipv6h->nexthdr = psph->nexthdr;
+		((u8 *)ipv6h)[nh_off] = psph->nexthdr;
 		ipv6h->payload_len = htons(ntohs(ipv6h->payload_len) - encap);
 	}
 
-- 
2.53.0-Meta