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 14int 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} 45EXPORT_SYMBOL_GPL(nsh_push); 46 47int 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} 74EXPORT_SYMBOL_GPL(nsh_pop); 75 76static 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 u16 mac_offset = skb->mac_header; 81 unsigned int nsh_len, mac_len; 82 __be16 proto; 83 84 skb_reset_network_header(skb); 85 86 mac_len = skb->mac_len; 87 88 if (unlikely(!pskb_may_pull(skb, NSH_BASE_HDR_LEN))) 89 goto out; 90 nsh_len = nsh_hdr_len(nsh_hdr(skb)); 91 if (nsh_len < NSH_BASE_HDR_LEN) 92 goto out; 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->mac_len = proto == htons(ETH_P_TEB) ? ETH_HLEN : 0; 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 mac_offset, mac_len); 111 goto out; 112 } 113 114 for (skb = segs; skb; skb = skb->next) { 115 skb->protocol = htons(ETH_P_NSH); 116 __skb_push(skb, nsh_len); 117 skb->mac_header = mac_offset; 118 skb->network_header = skb->mac_header + mac_len; 119 skb->mac_len = mac_len; 120 } 121 122out: 123 return segs; 124} 125 126static struct packet_offload nsh_packet_offload __read_mostly = { 127 .type = htons(ETH_P_NSH), 128 .priority = 15, 129 .callbacks = { 130 .gso_segment = nsh_gso_segment, 131 }, 132}; 133 134static int __init nsh_init_module(void) 135{ 136 dev_add_offload(&nsh_packet_offload); 137 return 0; 138} 139 140static void __exit nsh_cleanup_module(void) 141{ 142 dev_remove_offload(&nsh_packet_offload); 143} 144 145module_init(nsh_init_module); 146module_exit(nsh_cleanup_module); 147 148MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>"); 149MODULE_DESCRIPTION("NSH protocol"); 150MODULE_LICENSE("GPL v2"); 151