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