net/nsh/nsh.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * Network Service Header
0004  *
0005  * Copyright (c) 2017 Red Hat, Inc. -- Jiri Benc <jbenc@redhat.com>
0006  */
0007
0008 #include <linux/module.h>
0009 #include <linux/netdevice.h>
0010 #include <linux/skbuff.h>
0011 #include <net/nsh.h>
0012 #include <net/tun_proto.h>
0013
0014 int nsh_push(struct sk_buff *skb, const struct nshhdr *pushed_nh)
0015 {
0016     struct nshhdr *nh;
0017     size_t length = nsh_hdr_len(pushed_nh);
0018     u8 next_proto;
0019
0020     if (skb->mac_len) {
0021         next_proto = TUN_P_ETHERNET;
0022     } else {
0023         next_proto = tun_p_from_eth_p(skb->protocol);
0024         if (!next_proto)
0025             return -EAFNOSUPPORT;
0026     }
0027
0028     /* Add the NSH header */
0029     if (skb_cow_head(skb, length) < 0)
0030         return -ENOMEM;
0031
0032     skb_push(skb, length);
0033     nh = (struct nshhdr *)(skb->data);
0034     memcpy(nh, pushed_nh, length);
0035     nh->np = next_proto;
0036     skb_postpush_rcsum(skb, nh, length);
0037
0038     skb->protocol = htons(ETH_P_NSH);
0039     skb_reset_mac_header(skb);
0040     skb_reset_network_header(skb);
0041     skb_reset_mac_len(skb);
0042
0043     return 0;
0044 }
0045 EXPORT_SYMBOL_GPL(nsh_push);
0046
0047 int nsh_pop(struct sk_buff *skb)
0048 {
0049     struct nshhdr *nh;
0050     size_t length;
0051     __be16 inner_proto;
0052
0053     if (!pskb_may_pull(skb, NSH_BASE_HDR_LEN))
0054         return -ENOMEM;
0055     nh = (struct nshhdr *)(skb->data);
0056     length = nsh_hdr_len(nh);
0057     if (length < NSH_BASE_HDR_LEN)
0058         return -EINVAL;
0059     inner_proto = tun_p_to_eth_p(nh->np);
0060     if (!pskb_may_pull(skb, length))
0061         return -ENOMEM;
0062
0063     if (!inner_proto)
0064         return -EAFNOSUPPORT;
0065
0066     skb_pull_rcsum(skb, length);
0067     skb_reset_mac_header(skb);
0068     skb_reset_network_header(skb);
0069     skb_reset_mac_len(skb);
0070     skb->protocol = inner_proto;
0071
0072     return 0;
0073 }
0074 EXPORT_SYMBOL_GPL(nsh_pop);
0075
0076 static struct sk_buff *nsh_gso_segment(struct sk_buff *skb,
0077                        netdev_features_t features)
0078 {
0079     struct sk_buff *segs = ERR_PTR(-EINVAL);
0080     unsigned int nsh_len, mac_len;
0081     __be16 proto;
0082     int nhoff;
0083
0084     skb_reset_network_header(skb);
0085
0086     nhoff = skb->network_header - skb->mac_header;
0087     mac_len = skb->mac_len;
0088
0089     if (unlikely(!pskb_may_pull(skb, NSH_BASE_HDR_LEN)))
0090         goto out;
0091     nsh_len = nsh_hdr_len(nsh_hdr(skb));
0092     if (nsh_len < NSH_BASE_HDR_LEN)
0093         goto out;
0094     if (unlikely(!pskb_may_pull(skb, nsh_len)))
0095         goto out;
0096
0097     proto = tun_p_to_eth_p(nsh_hdr(skb)->np);
0098     if (!proto)
0099         goto out;
0100
0101     __skb_pull(skb, nsh_len);
0102
0103     skb_reset_mac_header(skb);
0104     skb->mac_len = proto == htons(ETH_P_TEB) ? ETH_HLEN : 0;
0105     skb->protocol = proto;
0106
0107     features &= NETIF_F_SG;
0108     segs = skb_mac_gso_segment(skb, features);
0109     if (IS_ERR_OR_NULL(segs)) {
0110         skb_gso_error_unwind(skb, htons(ETH_P_NSH), nsh_len,
0111                      skb->network_header - nhoff,
0112                      mac_len);
0113         goto out;
0114     }
0115
0116     for (skb = segs; skb; skb = skb->next) {
0117         skb->protocol = htons(ETH_P_NSH);
0118         __skb_push(skb, nsh_len);
0119         skb_set_mac_header(skb, -nhoff);
0120         skb->network_header = skb->mac_header + mac_len;
0121         skb->mac_len = mac_len;
0122     }
0123
0124 out:
0125     return segs;
0126 }
0127
0128 static struct packet_offload nsh_packet_offload __read_mostly = {
0129     .type = htons(ETH_P_NSH),
0130     .priority = 15,
0131     .callbacks = {
0132         .gso_segment = nsh_gso_segment,
0133     },
0134 };
0135
0136 static int __init nsh_init_module(void)
0137 {
0138     dev_add_offload(&nsh_packet_offload);
0139     return 0;
0140 }
0141
0142 static void __exit nsh_cleanup_module(void)
0143 {
0144     dev_remove_offload(&nsh_packet_offload);
0145 }
0146
0147 module_init(nsh_init_module);
0148 module_exit(nsh_cleanup_module);
0149
0150 MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
0151 MODULE_DESCRIPTION("NSH protocol");
0152 MODULE_LICENSE("GPL v2");