diff --git a/net/psp/psp_main.c b/net/psp/psp_main.c
index c9c1a8826b7f..0c009d7b08c0 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,42 @@ 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 -1 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_ROUTING)
+ return -1;
+ rem = len - off;
+ if (rem < sizeof(*eh))
+ return -1;
+
+ nh = off + offsetof(struct ipv6_opt_hdr, nexthdr);
+ if (type == NEXTHDR_AUTH)
+ off += ipv6_authlen(eh);
+ else
+ off += ipv6_optlen(eh);
+ }
+
+ return off == len ? nh : -1;
+}
+
static void psp_write_headers(struct net *net, struct sk_buff *skb, __be32 spi,
u8 ver, unsigned int udp_len, __be16 sport)
{@@ -243,7 +280,7 @@ static void psp_write_headers(struct net *net, struct sk_buff *skb, __be32 spi,
}
/* Encapsulate a TCP packet with PSP by adding the UDP+PSP headers and filling
- * them in.
+ * them in. With IPv6, they go after any extension headers.
*/
bool psp_dev_encapsulate(struct net *net, struct sk_buff *skb, __be32 spi,
u8 ver, __be16 sport)
@@ -251,10 +288,17 @@ bool psp_dev_encapsulate(struct net *net, struct sk_buff *skb, __be32 spi,
u32 network_len = skb_network_header_len(skb);
u32 ethr_len = skb_mac_header_len(skb);
u32 bufflen = ethr_len + network_len;
-
- if (skb->protocol != htons(ETH_P_IP) &&
- skb->protocol != htons(ETH_P_IPV6))
+ int nh_off = 0;
+
+ if (skb->protocol == htons(ETH_P_IPV6)) {
+ /* UDP + PSP after the extension headers, if any */
+ nh_off = psp_ipv6_nexthdr_off(skb_network_header(skb),
+ network_len);
+ if (nh_off < 0)
+ return false;
+ } else if (skb->protocol != htons(ETH_P_IP)) {
return false;
+ }
if (skb_cow_head(skb, PSP_ENCAP_HLEN))
return false;@@ -272,7 +316,7 @@ bool psp_dev_encapsulate(struct net *net, struct sk_buff *skb, __be32 spi,
ip_hdr(skb)->check =
ip_fast_csum((u8 *)ip_hdr(skb), ip_hdr(skb)->ihl);
} else {
- ipv6_hdr(skb)->nexthdr = IPPROTO_UDP;
+ skb_network_header(skb)[nh_off] = IPPROTO_UDP;
be16_add_cpu(&ipv6_hdr(skb)->payload_len, PSP_ENCAP_HLEN);
}
@@ -331,8 +375,23 @@ 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;
- is_udp = ipv6h->nexthdr == IPPROTO_UDP;
+ /* UDP + PSP come after any extension headers. Fragment and
+ * routing headers are not supported in transport mode.
+ */
+ off = ipv6_skip_exthdr(skb, l2_hlen + sizeof(*ipv6h), &nexthdr,
+ &frag_off);
+ if (unlikely(off < 0 || frag_off))
+ 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;
}
if (unlikely(!is_udp))@@ -386,11 +445,14 @@ int psp_dev_rcv(struct sk_buff *skb, u16 dev_id, u8 generation, bool strip_icv)
iph->check = ip_fast_csum((u8 *)iph, iph->ihl);
} else {
struct ipv6hdr *ipv6h = (struct ipv6hdr *)(skb->data + l2_hlen);
+ int nh_off = psp_ipv6_nexthdr_off((u8 *)ipv6h, l3_hlen);
- if (unlikely(ntohs(ipv6h->payload_len) < encap))
+ if (unlikely(nh_off < 0 ||
+ 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