1d2912cb1SThomas Gleixner // SPDX-License-Identifier: GPL-2.0-only 2c411ed85SJiri Benc /* 3c411ed85SJiri Benc * Network Service Header 4c411ed85SJiri Benc * 5c411ed85SJiri Benc * Copyright (c) 2017 Red Hat, Inc. -- Jiri Benc <jbenc@redhat.com> 6c411ed85SJiri Benc */ 7c411ed85SJiri Benc 8c411ed85SJiri Benc #include <linux/module.h> 9c411ed85SJiri Benc #include <linux/netdevice.h> 10c411ed85SJiri Benc #include <linux/skbuff.h> 11c411ed85SJiri Benc #include <net/nsh.h> 12c411ed85SJiri Benc #include <net/tun_proto.h> 13c411ed85SJiri Benc 14b2d0f5d5SYi Yang int nsh_push(struct sk_buff *skb, const struct nshhdr *pushed_nh) 15b2d0f5d5SYi Yang { 16b2d0f5d5SYi Yang struct nshhdr *nh; 17b2d0f5d5SYi Yang size_t length = nsh_hdr_len(pushed_nh); 18b2d0f5d5SYi Yang u8 next_proto; 19b2d0f5d5SYi Yang 20b2d0f5d5SYi Yang if (skb->mac_len) { 21b2d0f5d5SYi Yang next_proto = TUN_P_ETHERNET; 22b2d0f5d5SYi Yang } else { 23b2d0f5d5SYi Yang next_proto = tun_p_from_eth_p(skb->protocol); 24b2d0f5d5SYi Yang if (!next_proto) 25b2d0f5d5SYi Yang return -EAFNOSUPPORT; 26b2d0f5d5SYi Yang } 27b2d0f5d5SYi Yang 28b2d0f5d5SYi Yang /* Add the NSH header */ 29b2d0f5d5SYi Yang if (skb_cow_head(skb, length) < 0) 30b2d0f5d5SYi Yang return -ENOMEM; 31b2d0f5d5SYi Yang 32b2d0f5d5SYi Yang skb_push(skb, length); 33b2d0f5d5SYi Yang nh = (struct nshhdr *)(skb->data); 34b2d0f5d5SYi Yang memcpy(nh, pushed_nh, length); 35b2d0f5d5SYi Yang nh->np = next_proto; 36b2d0f5d5SYi Yang skb_postpush_rcsum(skb, nh, length); 37b2d0f5d5SYi Yang 38b2d0f5d5SYi Yang skb->protocol = htons(ETH_P_NSH); 39b2d0f5d5SYi Yang skb_reset_mac_header(skb); 40b2d0f5d5SYi Yang skb_reset_network_header(skb); 41b2d0f5d5SYi Yang skb_reset_mac_len(skb); 42b2d0f5d5SYi Yang 43b2d0f5d5SYi Yang return 0; 44b2d0f5d5SYi Yang } 45b2d0f5d5SYi Yang EXPORT_SYMBOL_GPL(nsh_push); 46b2d0f5d5SYi Yang 47b2d0f5d5SYi Yang int nsh_pop(struct sk_buff *skb) 48b2d0f5d5SYi Yang { 49b2d0f5d5SYi Yang struct nshhdr *nh; 50b2d0f5d5SYi Yang size_t length; 51b2d0f5d5SYi Yang __be16 inner_proto; 52b2d0f5d5SYi Yang 53b2d0f5d5SYi Yang if (!pskb_may_pull(skb, NSH_BASE_HDR_LEN)) 54b2d0f5d5SYi Yang return -ENOMEM; 55b2d0f5d5SYi Yang nh = (struct nshhdr *)(skb->data); 56b2d0f5d5SYi Yang length = nsh_hdr_len(nh); 57af50e4baSEric Dumazet if (length < NSH_BASE_HDR_LEN) 58af50e4baSEric Dumazet return -EINVAL; 59b2d0f5d5SYi Yang inner_proto = tun_p_to_eth_p(nh->np); 60b2d0f5d5SYi Yang if (!pskb_may_pull(skb, length)) 61b2d0f5d5SYi Yang return -ENOMEM; 62b2d0f5d5SYi Yang 63b2d0f5d5SYi Yang if (!inner_proto) 64b2d0f5d5SYi Yang return -EAFNOSUPPORT; 65b2d0f5d5SYi Yang 66b2d0f5d5SYi Yang skb_pull_rcsum(skb, length); 67b2d0f5d5SYi Yang skb_reset_mac_header(skb); 68b2d0f5d5SYi Yang skb_reset_network_header(skb); 69b2d0f5d5SYi Yang skb_reset_mac_len(skb); 70b2d0f5d5SYi Yang skb->protocol = inner_proto; 71b2d0f5d5SYi Yang 72b2d0f5d5SYi Yang return 0; 73b2d0f5d5SYi Yang } 74b2d0f5d5SYi Yang EXPORT_SYMBOL_GPL(nsh_pop); 75b2d0f5d5SYi Yang 76c411ed85SJiri Benc static struct sk_buff *nsh_gso_segment(struct sk_buff *skb, 77c411ed85SJiri Benc netdev_features_t features) 78c411ed85SJiri Benc { 79c411ed85SJiri Benc struct sk_buff *segs = ERR_PTR(-EINVAL); 80*c83b4938SDong Chenchen u16 mac_offset = skb->mac_header; 81c411ed85SJiri Benc unsigned int nsh_len, mac_len; 82c411ed85SJiri Benc __be16 proto; 83c411ed85SJiri Benc 84c411ed85SJiri Benc skb_reset_network_header(skb); 85c411ed85SJiri Benc 86c411ed85SJiri Benc mac_len = skb->mac_len; 87c411ed85SJiri Benc 88c411ed85SJiri Benc if (unlikely(!pskb_may_pull(skb, NSH_BASE_HDR_LEN))) 89c411ed85SJiri Benc goto out; 90c411ed85SJiri Benc nsh_len = nsh_hdr_len(nsh_hdr(skb)); 91af50e4baSEric Dumazet if (nsh_len < NSH_BASE_HDR_LEN) 92af50e4baSEric Dumazet goto out; 93c411ed85SJiri Benc if (unlikely(!pskb_may_pull(skb, nsh_len))) 94c411ed85SJiri Benc goto out; 95c411ed85SJiri Benc 96c411ed85SJiri Benc proto = tun_p_to_eth_p(nsh_hdr(skb)->np); 97c411ed85SJiri Benc if (!proto) 98c411ed85SJiri Benc goto out; 99c411ed85SJiri Benc 100c411ed85SJiri Benc __skb_pull(skb, nsh_len); 101c411ed85SJiri Benc 102c411ed85SJiri Benc skb_reset_mac_header(skb); 103bab2c80eSWillem de Bruijn skb->mac_len = proto == htons(ETH_P_TEB) ? ETH_HLEN : 0; 104c411ed85SJiri Benc skb->protocol = proto; 105c411ed85SJiri Benc 106c411ed85SJiri Benc features &= NETIF_F_SG; 107c411ed85SJiri Benc segs = skb_mac_gso_segment(skb, features); 108c411ed85SJiri Benc if (IS_ERR_OR_NULL(segs)) { 109c411ed85SJiri Benc skb_gso_error_unwind(skb, htons(ETH_P_NSH), nsh_len, 110*c83b4938SDong Chenchen mac_offset, mac_len); 111c411ed85SJiri Benc goto out; 112c411ed85SJiri Benc } 113c411ed85SJiri Benc 114c411ed85SJiri Benc for (skb = segs; skb; skb = skb->next) { 115c411ed85SJiri Benc skb->protocol = htons(ETH_P_NSH); 116c411ed85SJiri Benc __skb_push(skb, nsh_len); 117*c83b4938SDong Chenchen skb->mac_header = mac_offset; 118c411ed85SJiri Benc skb->network_header = skb->mac_header + mac_len; 119c411ed85SJiri Benc skb->mac_len = mac_len; 120c411ed85SJiri Benc } 121c411ed85SJiri Benc 122c411ed85SJiri Benc out: 123c411ed85SJiri Benc return segs; 124c411ed85SJiri Benc } 125c411ed85SJiri Benc 126c411ed85SJiri Benc static struct packet_offload nsh_packet_offload __read_mostly = { 127c411ed85SJiri Benc .type = htons(ETH_P_NSH), 128c411ed85SJiri Benc .priority = 15, 129c411ed85SJiri Benc .callbacks = { 130c411ed85SJiri Benc .gso_segment = nsh_gso_segment, 131c411ed85SJiri Benc }, 132c411ed85SJiri Benc }; 133c411ed85SJiri Benc 134c411ed85SJiri Benc static int __init nsh_init_module(void) 135c411ed85SJiri Benc { 136c411ed85SJiri Benc dev_add_offload(&nsh_packet_offload); 137c411ed85SJiri Benc return 0; 138c411ed85SJiri Benc } 139c411ed85SJiri Benc 140c411ed85SJiri Benc static void __exit nsh_cleanup_module(void) 141c411ed85SJiri Benc { 142c411ed85SJiri Benc dev_remove_offload(&nsh_packet_offload); 143c411ed85SJiri Benc } 144c411ed85SJiri Benc 145c411ed85SJiri Benc module_init(nsh_init_module); 146c411ed85SJiri Benc module_exit(nsh_cleanup_module); 147c411ed85SJiri Benc 148c411ed85SJiri Benc MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>"); 149c411ed85SJiri Benc MODULE_DESCRIPTION("NSH protocol"); 150c411ed85SJiri Benc MODULE_LICENSE("GPL v2"); 151