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 14 int 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 } 45 EXPORT_SYMBOL_GPL(nsh_push); 46 47 int 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 } 74 EXPORT_SYMBOL_GPL(nsh_pop); 75 76 static 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 124 out: 125 return segs; 126 } 127 128 static 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 136 static int __init nsh_init_module(void) 137 { 138 dev_add_offload(&nsh_packet_offload); 139 return 0; 140 } 141 142 static void __exit nsh_cleanup_module(void) 143 { 144 dev_remove_offload(&nsh_packet_offload); 145 } 146 147 module_init(nsh_init_module); 148 module_exit(nsh_cleanup_module); 149 150 MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>"); 151 MODULE_DESCRIPTION("NSH protocol"); 152 MODULE_LICENSE("GPL v2"); 153