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 inner_proto = tun_p_to_eth_p(nh->np); 61 if (!pskb_may_pull(skb, length)) 62 return -ENOMEM; 63 64 if (!inner_proto) 65 return -EAFNOSUPPORT; 66 67 skb_pull_rcsum(skb, length); 68 skb_reset_mac_header(skb); 69 skb_reset_network_header(skb); 70 skb_reset_mac_len(skb); 71 skb->protocol = inner_proto; 72 73 return 0; 74 } 75 EXPORT_SYMBOL_GPL(nsh_pop); 76 77 static struct sk_buff *nsh_gso_segment(struct sk_buff *skb, 78 netdev_features_t features) 79 { 80 struct sk_buff *segs = ERR_PTR(-EINVAL); 81 unsigned int nsh_len, mac_len; 82 __be16 proto; 83 int nhoff; 84 85 skb_reset_network_header(skb); 86 87 nhoff = skb->network_header - skb->mac_header; 88 mac_len = skb->mac_len; 89 90 if (unlikely(!pskb_may_pull(skb, NSH_BASE_HDR_LEN))) 91 goto out; 92 nsh_len = nsh_hdr_len(nsh_hdr(skb)); 93 if (unlikely(!pskb_may_pull(skb, nsh_len))) 94 goto out; 95 96 proto = tun_p_to_eth_p(nsh_hdr(skb)->np); 97 if (!proto) 98 goto out; 99 100 __skb_pull(skb, nsh_len); 101 102 skb_reset_mac_header(skb); 103 skb_reset_mac_len(skb); 104 skb->protocol = proto; 105 106 features &= NETIF_F_SG; 107 segs = skb_mac_gso_segment(skb, features); 108 if (IS_ERR_OR_NULL(segs)) { 109 skb_gso_error_unwind(skb, htons(ETH_P_NSH), nsh_len, 110 skb->network_header - nhoff, 111 mac_len); 112 goto out; 113 } 114 115 for (skb = segs; skb; skb = skb->next) { 116 skb->protocol = htons(ETH_P_NSH); 117 __skb_push(skb, nsh_len); 118 skb_set_mac_header(skb, -nhoff); 119 skb->network_header = skb->mac_header + mac_len; 120 skb->mac_len = mac_len; 121 } 122 123 out: 124 return segs; 125 } 126 127 static struct packet_offload nsh_packet_offload __read_mostly = { 128 .type = htons(ETH_P_NSH), 129 .priority = 15, 130 .callbacks = { 131 .gso_segment = nsh_gso_segment, 132 }, 133 }; 134 135 static int __init nsh_init_module(void) 136 { 137 dev_add_offload(&nsh_packet_offload); 138 return 0; 139 } 140 141 static void __exit nsh_cleanup_module(void) 142 { 143 dev_remove_offload(&nsh_packet_offload); 144 } 145 146 module_init(nsh_init_module); 147 module_exit(nsh_cleanup_module); 148 149 MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>"); 150 MODULE_DESCRIPTION("NSH protocol"); 151 MODULE_LICENSE("GPL v2"); 152