net/nsh/nsh.c

   1 /*
   2  * Network Service Header
   3  *
   4  * Copyright (c) 2017 Red Hat, Inc. -- Jiri Benc <jbenc@redhat.com>
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License version 2 as
   8  * published by the Free Software Foundation.
   9  */
  10
  11 #include <linux/module.h>
  12 #include <linux/netdevice.h>
  13 #include <linux/skbuff.h>
  14 #include <net/nsh.h>
  15 #include <net/tun_proto.h>
  16
  17 int nsh_push(struct sk_buff *skb, const struct nshhdr *pushed_nh)
  18 {
  19         struct nshhdr *nh;
  20         size_t length = nsh_hdr_len(pushed_nh);
  21         u8 next_proto;
  22
  23         if (skb->mac_len) {
  24                 next_proto = TUN_P_ETHERNET;
  25         } else {
  26                 next_proto = tun_p_from_eth_p(skb->protocol);
  27                 if (!next_proto)
  28                         return -EAFNOSUPPORT;
  29         }
  30
  31         /* Add the NSH header */
  32         if (skb_cow_head(skb, length) < 0)
  33                 return -ENOMEM;
  34
  35         skb_push(skb, length);
  36         nh = (struct nshhdr *)(skb->data);
  37         memcpy(nh, pushed_nh, length);
  38         nh->np = next_proto;
  39         skb_postpush_rcsum(skb, nh, length);
  40
  41         skb->protocol = htons(ETH_P_NSH);
  42         skb_reset_mac_header(skb);
  43         skb_reset_network_header(skb);
  44         skb_reset_mac_len(skb);
  45
  46         return 0;
  47 }
  48 EXPORT_SYMBOL_GPL(nsh_push);
  49
  50 int nsh_pop(struct sk_buff *skb)
  51 {
  52         struct nshhdr *nh;
  53         size_t length;
  54         __be16 inner_proto;
  55
  56         if (!pskb_may_pull(skb, NSH_BASE_HDR_LEN))
  57                 return -ENOMEM;
  58         nh = (struct nshhdr *)(skb->data);
  59         length = nsh_hdr_len(nh);
  60         if (length < NSH_BASE_HDR_LEN)
  61                 return -EINVAL;
  62         inner_proto = tun_p_to_eth_p(nh->np);
  63         if (!pskb_may_pull(skb, length))
  64                 return -ENOMEM;
  65
  66         if (!inner_proto)
  67                 return -EAFNOSUPPORT;
  68
  69         skb_pull_rcsum(skb, length);
  70         skb_reset_mac_header(skb);
  71         skb_reset_network_header(skb);
  72         skb_reset_mac_len(skb);
  73         skb->protocol = inner_proto;
  74
  75         return 0;
  76 }
  77 EXPORT_SYMBOL_GPL(nsh_pop);
  78
  79 static struct sk_buff *nsh_gso_segment(struct sk_buff *skb,
  80                                        netdev_features_t features)
  81 {
  82         struct sk_buff *segs = ERR_PTR(-EINVAL);
  83         unsigned int nsh_len, mac_len;
  84         __be16 proto;
  85         int nhoff;
  86
  87         skb_reset_network_header(skb);
  88
  89         nhoff = skb->network_header - skb->mac_header;
  90         mac_len = skb->mac_len;
  91
  92         if (unlikely(!pskb_may_pull(skb, NSH_BASE_HDR_LEN)))
  93                 goto out;
  94         nsh_len = nsh_hdr_len(nsh_hdr(skb));
  95         if (nsh_len < NSH_BASE_HDR_LEN)
  96                 goto out;
  97         if (unlikely(!pskb_may_pull(skb, nsh_len)))
  98                 goto out;
  99
 100         proto = tun_p_to_eth_p(nsh_hdr(skb)->np);
 101         if (!proto)
 102                 goto out;
 103
 104         __skb_pull(skb, nsh_len);
 105
 106         skb_reset_mac_header(skb);
 107         skb->mac_len = proto == htons(ETH_P_TEB) ? ETH_HLEN : 0;
 108         skb->protocol = proto;
 109
 110         features &= NETIF_F_SG;
 111         segs = skb_mac_gso_segment(skb, features);
 112         if (IS_ERR_OR_NULL(segs)) {
 113                 skb_gso_error_unwind(skb, htons(ETH_P_NSH), nsh_len,
 114                                      skb->network_header - nhoff,
 115                                      mac_len);
 116                 goto out;
 117         }
 118
 119         for (skb = segs; skb; skb = skb->next) {
 120                 skb->protocol = htons(ETH_P_NSH);
 121                 __skb_push(skb, nsh_len);
 122                 skb_set_mac_header(skb, -nhoff);
 123                 skb->network_header = skb->mac_header + mac_len;
 124                 skb->mac_len = mac_len;
 125         }
 126
 127 out:
 128         return segs;
 129 }
 130
 131 static struct packet_offload nsh_packet_offload __read_mostly = {
 132         .type = htons(ETH_P_NSH),
 133         .priority = 15,
 134         .callbacks = {
 135                 .gso_segment = nsh_gso_segment,
 136         },
 137 };
 138
 139 static int __init nsh_init_module(void)
 140 {
 141         dev_add_offload(&nsh_packet_offload);
 142         return 0;
 143 }
 144
 145 static void __exit nsh_cleanup_module(void)
 146 {
 147         dev_remove_offload(&nsh_packet_offload);
 148 }
 149
 150 module_init(nsh_init_module);
 151 module_exit(nsh_cleanup_module);
 152
 153 MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
 154 MODULE_DESCRIPTION("NSH protocol");
 155 MODULE_LICENSE("GPL v2");