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 unsigned int outer_hlen, mac_len, nsh_len; 81 struct sk_buff *segs = ERR_PTR(-EINVAL); 82 u16 mac_offset = skb->mac_header; 83 __be16 outer_proto, proto; 84 85 skb_reset_network_header(skb); 86 87 outer_proto = skb->protocol; 88 outer_hlen = skb_mac_header_len(skb); 89 mac_len = skb->mac_len; 90 91 if (unlikely(!pskb_may_pull(skb, NSH_BASE_HDR_LEN))) 92 goto out; 93 nsh_len = nsh_hdr_len(nsh_hdr(skb)); 94 if (nsh_len < NSH_BASE_HDR_LEN) 95 goto out; 96 if (unlikely(!pskb_may_pull(skb, nsh_len))) 97 goto out; 98 99 proto = tun_p_to_eth_p(nsh_hdr(skb)->np); 100 if (!proto) 101 goto out; 102 103 __skb_pull(skb, nsh_len); 104 105 skb_reset_mac_header(skb); 106 skb->mac_len = proto == htons(ETH_P_TEB) ? ETH_HLEN : 0; 107 skb->protocol = proto; 108 109 features &= NETIF_F_SG; 110 segs = skb_mac_gso_segment(skb, features); 111 if (IS_ERR_OR_NULL(segs)) { 112 skb_gso_error_unwind(skb, htons(ETH_P_NSH), nsh_len, 113 mac_offset, mac_len); 114 goto out; 115 } 116 117 for (skb = segs; skb; skb = skb->next) { 118 skb->protocol = outer_proto; 119 __skb_push(skb, nsh_len + outer_hlen); 120 skb_reset_mac_header(skb); 121 skb_set_network_header(skb, outer_hlen); 122 skb->mac_len = mac_len; 123 } 124 125 out: 126 return segs; 127 } 128 129 static struct packet_offload nsh_packet_offload __read_mostly = { 130 .type = htons(ETH_P_NSH), 131 .priority = 15, 132 .callbacks = { 133 .gso_segment = nsh_gso_segment, 134 }, 135 }; 136 137 static int __init nsh_init_module(void) 138 { 139 dev_add_offload(&nsh_packet_offload); 140 return 0; 141 } 142 143 static void __exit nsh_cleanup_module(void) 144 { 145 dev_remove_offload(&nsh_packet_offload); 146 } 147 148 module_init(nsh_init_module); 149 module_exit(nsh_cleanup_module); 150 151 MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>"); 152 MODULE_DESCRIPTION("NSH protocol"); 153 MODULE_LICENSE("GPL v2"); 154