From: Raed Salem Create a new function psp_encapsulate(), which takes a TCP packet and PSP encapsulates it according to the "Transport Mode Packet Format" section of the PSP Architecture Specification. psp_encapsulate() does not push a PSP trailer onto the skb. Only IPv6 is supported. Virtualization cookie is not included. Reviewed-by: Willem de Bruijn Signed-off-by: Raed Salem Signed-off-by: Rahul Rameshbabu Signed-off-by: Cosmin Ratiu Signed-off-by: Daniel Zahka --- Notes: v4: - rename psp_encapsulate() to psp_dev_encapsulate() v3: - patch introduced include/net/psp/functions.h | 2 ++ include/net/psp/types.h | 2 ++ net/psp/psp_main.c | 57 +++++++++++++++++++++++++++++++++++++ 3 files changed, 61 insertions(+) diff --git a/include/net/psp/functions.h b/include/net/psp/functions.h index 17642c944620..460af903a35a 100644 --- a/include/net/psp/functions.h +++ b/include/net/psp/functions.h @@ -16,6 +16,8 @@ struct psp_dev * psp_dev_create(struct net_device *netdev, struct psp_dev_ops *psd_ops, struct psp_dev_caps *psd_caps, void *priv_ptr); void psp_dev_unregister(struct psp_dev *psd); +bool psp_dev_encapsulate(struct net *net, struct sk_buff *skb, + __be32 spi, u8 ver, __be16 sport); /* Kernel-facing API */ void psp_assoc_put(struct psp_assoc *pas); diff --git a/include/net/psp/types.h b/include/net/psp/types.h index ec218747ced0..d9688e66cf09 100644 --- a/include/net/psp/types.h +++ b/include/net/psp/types.h @@ -20,6 +20,8 @@ struct psphdr { __be64 vc[]; /* optional */ }; +#define PSP_ENCAP_HLEN (sizeof(struct udphdr) + sizeof(struct psphdr)) + #define PSP_SPI_KEY_ID GENMASK(30, 0) #define PSP_SPI_KEY_PHASE BIT(31) diff --git a/net/psp/psp_main.c b/net/psp/psp_main.c index 0fdfe6f65f87..434ff6dac18f 100644 --- a/net/psp/psp_main.c +++ b/net/psp/psp_main.c @@ -1,10 +1,12 @@ // SPDX-License-Identifier: GPL-2.0-only +#include #include #include #include #include #include +#include #include "psp.h" #include "psp-nl-gen.h" @@ -153,6 +155,61 @@ unsigned int psp_key_size(u32 version) } EXPORT_SYMBOL(psp_key_size); +static void psp_write_headers(struct net *net, struct sk_buff *skb, + __be32 spi, u8 ver, unsigned int udp_len, + __be16 sport) +{ + struct udphdr *uh = udp_hdr(skb); + struct psphdr *psph = (struct psphdr *)(uh + 1); + + uh->dest = htons(PSP_DEFAULT_UDP_PORT); + uh->source = udp_flow_src_port(net, skb, 0, 0, false); + uh->check = 0; + uh->len = htons(udp_len); + + psph->nexthdr = IPPROTO_TCP; + psph->hdrlen = PSP_HDRLEN_NOOPT; + psph->crypt_offset = 0; + psph->verfl = FIELD_PREP(PSPHDR_VERFL_VERSION, ver) | + FIELD_PREP(PSPHDR_VERFL_ONE, 1); + psph->spi = spi; + memset(&psph->iv, 0, sizeof(psph->iv)); +} + +/* Encapsulate a TCP packet with PSP by adding the UDP+PSP headers and filling + * them in. + */ +bool psp_dev_encapsulate(struct net *net, struct sk_buff *skb, + __be32 spi, u8 ver, __be16 sport) +{ + u32 network_len = skb_network_header_len(skb); + u32 ethr_len = skb_mac_header_len(skb); + u32 bufflen = ethr_len + network_len; + struct ipv6hdr *ip6; + + if (skb_cow_head(skb, PSP_ENCAP_HLEN)) + return false; + + skb_push(skb, PSP_ENCAP_HLEN); + skb->mac_header -= PSP_ENCAP_HLEN; + skb->network_header -= PSP_ENCAP_HLEN; + skb->transport_header -= PSP_ENCAP_HLEN; + memmove(skb->data, skb->data + PSP_ENCAP_HLEN, bufflen); + + ip6 = ipv6_hdr(skb); + skb_set_inner_ipproto(skb, IPPROTO_TCP); + ip6->nexthdr = IPPROTO_UDP; + be16_add_cpu(&ip6->payload_len, PSP_ENCAP_HLEN); + + skb_set_inner_transport_header(skb, skb_transport_offset(skb) + PSP_ENCAP_HLEN); + skb->encapsulation = 1; + psp_write_headers(net, skb, spi, ver, + skb->len - skb_transport_offset(skb), sport); + + return true; +} +EXPORT_SYMBOL(psp_dev_encapsulate); + static int __init psp_init(void) { mutex_init(&psp_devs_lock); -- 2.47.1