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 static struct sk_buff *nsh_gso_segment(struct sk_buff *skb,
  18                                        netdev_features_t features)
  19 {
  20         struct sk_buff *segs = ERR_PTR(-EINVAL);
  21         u16 mac_offset = skb->mac_header;
  22         unsigned int nsh_len, mac_len;
  23         __be16 proto;
  24
  25         skb_reset_network_header(skb);
  26
  27         mac_len = skb->mac_len;
  28
  29         if (unlikely(!pskb_may_pull(skb, NSH_BASE_HDR_LEN)))
  30                 goto out;
  31         nsh_len = nsh_hdr_len(nsh_hdr(skb));
  32         if (nsh_len < NSH_BASE_HDR_LEN)
  33                 goto out;
  34         if (unlikely(!pskb_may_pull(skb, nsh_len)))
  35                 goto out;
  36
  37         proto = tun_p_to_eth_p(nsh_hdr(skb)->np);
  38         if (!proto)
  39                 goto out;
  40
  41         __skb_pull(skb, nsh_len);
  42
  43         skb_reset_mac_header(skb);
  44         skb->mac_len = proto == htons(ETH_P_TEB) ? ETH_HLEN : 0;
  45         skb->protocol = proto;
  46
  47         features &= NETIF_F_SG;
  48         segs = skb_mac_gso_segment(skb, features);
  49         if (IS_ERR_OR_NULL(segs)) {
  50                 skb_gso_error_unwind(skb, htons(ETH_P_NSH), nsh_len,
  51                                      mac_offset, mac_len);
  52                 goto out;
  53         }
  54
  55         for (skb = segs; skb; skb = skb->next) {
  56                 skb->protocol = htons(ETH_P_NSH);
  57                 __skb_push(skb, nsh_len);
  58                 skb->mac_header = mac_offset;
  59                 skb->network_header = skb->mac_header + mac_len;
  60                 skb->mac_len = mac_len;
  61         }
  62
  63 out:
  64         return segs;
  65 }
  66
  67 static struct packet_offload nsh_packet_offload __read_mostly = {
  68         .type = htons(ETH_P_NSH),
  69         .priority = 15,
  70         .callbacks = {
  71                 .gso_segment = nsh_gso_segment,
  72         },
  73 };
  74
  75 static int __init nsh_init_module(void)
  76 {
  77         dev_add_offload(&nsh_packet_offload);
  78         return 0;
  79 }
  80
  81 static void __exit nsh_cleanup_module(void)
  82 {
  83         dev_remove_offload(&nsh_packet_offload);
  84 }
  85
  86 module_init(nsh_init_module);
  87 module_exit(nsh_cleanup_module);
  88
  89 MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
  90 MODULE_DESCRIPTION("NSH protocol");
  91 MODULE_LICENSE("GPL v2");