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/gso.h> 12 #include <net/nsh.h> 13 #include <net/tun_proto.h> 14 15 int nsh_push(struct sk_buff *skb, const struct nshhdr *pushed_nh) 16 { 17 struct nshhdr *nh; 18 size_t length = nsh_hdr_len(pushed_nh); 19 u8 next_proto; 20 21 if (skb->mac_len) { 22 next_proto = TUN_P_ETHERNET; 23 } else { 24 next_proto = tun_p_from_eth_p(skb->protocol); 25 if (!next_proto) 26 return -EAFNOSUPPORT; 27 } 28 29 /* Add the NSH header */ 30 if (skb_cow_head(skb, length) < 0) 31 return -ENOMEM; 32 33 skb_push(skb, length); 34 nh = (struct nshhdr *)(skb->data); 35 memcpy(nh, pushed_nh, length); 36 nh->np = next_proto; 37 skb_postpush_rcsum(skb, nh, length); 38 39 skb->protocol = htons(ETH_P_NSH); 40 skb_reset_mac_header(skb); 41 skb_reset_network_header(skb); 42 skb_reset_mac_len(skb); 43 44 return 0; 45 } 46 EXPORT_SYMBOL_GPL(nsh_push); 47 48 int nsh_pop(struct sk_buff *skb) 49 { 50 struct nshhdr *nh; 51 size_t length; 52 __be16 inner_proto; 53 54 if (!pskb_may_pull(skb, NSH_BASE_HDR_LEN)) 55 return -ENOMEM; 56 nh = (struct nshhdr *)(skb->data); 57 length = nsh_hdr_len(nh); 58 if (length < NSH_BASE_HDR_LEN) 59 return -EINVAL; 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 u16 mac_offset = skb->mac_header; 82 unsigned int nsh_len, mac_len; 83 __be16 proto; 84 85 skb_reset_network_header(skb); 86 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 mac_offset, 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->mac_header = mac_offset; 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