net/nsh/nsh.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Network Service Header
   4  *
   5  * Copyright (c) 2017 Red Hat, Inc. -- Jiri Benc <[email protected]>
   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 <[email protected]>");
 152 MODULE_DESCRIPTION("NSH protocol");
 153 MODULE_LICENSE("GPL v2");