152 lines
3.2 KiB
C
152 lines
3.2 KiB
C
|
/*
|
||
|
|
* Network Service Header
|
||
|
|
*
|
||
|
|
* Copyright (c) 2017 Red Hat, Inc. -- Jiri Benc <jbenc@redhat.com>
|
||
|
|
*
|
||
|
|
* This program is free software; you can redistribute it and/or modify
|
||
|
|
* it under the terms of the GNU General Public License version 2 as
|
||
|
|
* published by the Free Software Foundation.
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <linux/module.h>
|
||
|
|
#include <linux/netdevice.h>
|
||
|
|
#include <linux/skbuff.h>
|
||
|
|
#include <net/nsh.h>
|
||
|
|
#include <net/tun_proto.h>
|
||
|
|
|
||
|
|
int nsh_push(struct sk_buff *skb, const struct nshhdr *pushed_nh)
|
||
|
|
{
|
||
|
|
struct nshhdr *nh;
|
||
|
|
size_t length = nsh_hdr_len(pushed_nh);
|
||
|
|
u8 next_proto;
|
||
|
|
|
||
|
|
if (skb->mac_len) {
|
||
|
|
next_proto = TUN_P_ETHERNET;
|
||
|
|
} else {
|
||
|
|
next_proto = tun_p_from_eth_p(skb->protocol);
|
||
|
|
if (!next_proto)
|
||
|
|
return -EAFNOSUPPORT;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Add the NSH header */
|
||
|
|
if (skb_cow_head(skb, length) < 0)
|
||
|
|
return -ENOMEM;
|
||
|
|
|
||
|
|
skb_push(skb, length);
|
||
|
|
nh = (struct nshhdr *)(skb->data);
|
||
|
|
memcpy(nh, pushed_nh, length);
|
||
|
|
nh->np = next_proto;
|
||
|
|
skb_postpush_rcsum(skb, nh, length);
|
||
|
|
|
||
|
|
skb->protocol = htons(ETH_P_NSH);
|
||
|
|
skb_reset_mac_header(skb);
|
||
|
|
skb_reset_network_header(skb);
|
||
|
|
skb_reset_mac_len(skb);
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
EXPORT_SYMBOL_GPL(nsh_push);
|
||
|
|
|
||
|
|
int nsh_pop(struct sk_buff *skb)
|
||
|
|
{
|
||
|
|
struct nshhdr *nh;
|
||
|
|
size_t length;
|
||
|
|
__be16 inner_proto;
|
||
|
|
|
||
|
|
if (!pskb_may_pull(skb, NSH_BASE_HDR_LEN))
|
||
|
|
return -ENOMEM;
|
||
|
|
nh = (struct nshhdr *)(skb->data);
|
||
|
|
length = nsh_hdr_len(nh);
|
||
|
|
inner_proto = tun_p_to_eth_p(nh->np);
|
||
|
|
if (!pskb_may_pull(skb, length))
|
||
|
|
return -ENOMEM;
|
||
|
|
|
||
|
|
if (!inner_proto)
|
||
|
|
return -EAFNOSUPPORT;
|
||
|
|
|
||
|
|
skb_pull_rcsum(skb, length);
|
||
|
|
skb_reset_mac_header(skb);
|
||
|
|
skb_reset_network_header(skb);
|
||
|
|
skb_reset_mac_len(skb);
|
||
|
|
skb->protocol = inner_proto;
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
EXPORT_SYMBOL_GPL(nsh_pop);
|
||
|
|
|
||
|
|
static struct sk_buff *nsh_gso_segment(struct sk_buff *skb,
|
||
|
|
netdev_features_t features)
|
||
|
|
{
|
||
|
|
struct sk_buff *segs = ERR_PTR(-EINVAL);
|
||
|
|
unsigned int nsh_len, mac_len;
|
||
|
|
__be16 proto;
|
||
|
|
int nhoff;
|
||
|
|
|
||
|
|
skb_reset_network_header(skb);
|
||
|
|
|
||
|
|
nhoff = skb->network_header - skb->mac_header;
|
||
|
|
mac_len = skb->mac_len;
|
||
|
|
|
||
|
|
if (unlikely(!pskb_may_pull(skb, NSH_BASE_HDR_LEN)))
|
||
|
|
goto out;
|
||
|
|
nsh_len = nsh_hdr_len(nsh_hdr(skb));
|
||
|
|
if (unlikely(!pskb_may_pull(skb, nsh_len)))
|
||
|
|
goto out;
|
||
|
|
|
||
|
|
proto = tun_p_to_eth_p(nsh_hdr(skb)->np);
|
||
|
|
if (!proto)
|
||
|
|
goto out;
|
||
|
|
|
||
|
|
__skb_pull(skb, nsh_len);
|
||
|
|
|
||
|
|
skb_reset_mac_header(skb);
|
||
|
|
skb_reset_mac_len(skb);
|
||
|
|
skb->protocol = proto;
|
||
|
|
|
||
|
|
features &= NETIF_F_SG;
|
||
|
|
segs = skb_mac_gso_segment(skb, features);
|
||
|
|
if (IS_ERR_OR_NULL(segs)) {
|
||
|
|
skb_gso_error_unwind(skb, htons(ETH_P_NSH), nsh_len,
|
||
|
|
skb->network_header - nhoff,
|
||
|
|
mac_len);
|
||
|
|
goto out;
|
||
|
|
}
|
||
|
|
|
||
|
|
for (skb = segs; skb; skb = skb->next) {
|
||
|
|
skb->protocol = htons(ETH_P_NSH);
|
||
|
|
__skb_push(skb, nsh_len);
|
||
|
|
skb_set_mac_header(skb, -nhoff);
|
||
|
|
skb->network_header = skb->mac_header + mac_len;
|
||
|
|
skb->mac_len = mac_len;
|
||
|
|
}
|
||
|
|
|
||
|
|
out:
|
||
|
|
return segs;
|
||
|
|
}
|
||
|
|
|
||
|
|
static struct packet_offload nsh_packet_offload __read_mostly = {
|
||
|
|
.type = htons(ETH_P_NSH),
|
||
|
|
.priority = 15,
|
||
|
|
.callbacks = {
|
||
|
|
.gso_segment = nsh_gso_segment,
|
||
|
|
},
|
||
|
|
};
|
||
|
|
|
||
|
|
static int __init nsh_init_module(void)
|
||
|
|
{
|
||
|
|
dev_add_offload(&nsh_packet_offload);
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
static void __exit nsh_cleanup_module(void)
|
||
|
|
{
|
||
|
|
dev_remove_offload(&nsh_packet_offload);
|
||
|
|
}
|
||
|
|
|
||
|
|
module_init(nsh_init_module);
|
||
|
|
module_exit(nsh_cleanup_module);
|
||
|
|
|
||
|
|
MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
|
||
|
|
MODULE_DESCRIPTION("NSH protocol");
|
||
|
|
MODULE_LICENSE("GPL v2");
|