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