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 <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
  14 int 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 }
  45 EXPORT_SYMBOL_GPL(nsh_push);
  46
  47 int 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 }
  74 EXPORT_SYMBOL_GPL(nsh_pop);
  75
  76 static 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
 122 out:
 123         return segs;
 124 }
 125
 126 static 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
 134 static int __init nsh_init_module(void)
 135 {
 136         dev_add_offload(&nsh_packet_offload);
 137         return 0;
 138 }
 139
 140 static void __exit nsh_cleanup_module(void)
 141 {
 142         dev_remove_offload(&nsh_packet_offload);
 143 }
 144
 145 module_init(nsh_init_module);
 146 module_exit(nsh_cleanup_module);
 147
 148 MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
 149 MODULE_DESCRIPTION("NSH protocol");
 150 MODULE_LICENSE("GPL v2");