LXR linux/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
  14int 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}
  45EXPORT_SYMBOL_GPL(nsh_push);
  46
  47int 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}
  74EXPORT_SYMBOL_GPL(nsh_pop);
  75
  76static 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        unsigned int nsh_len, mac_len;
  81        __be16 proto;
  82        int nhoff;
  83
  84        skb_reset_network_header(skb);
  85
  86        nhoff = skb->network_header - skb->mac_header;
  87        mac_len = skb->mac_len;
  88
  89        if (unlikely(!pskb_may_pull(skb, NSH_BASE_HDR_LEN)))
  90                goto out;
  91        nsh_len = nsh_hdr_len(nsh_hdr(skb));
  92        if (nsh_len < NSH_BASE_HDR_LEN)
  93                goto out;
  94        if (unlikely(!pskb_may_pull(skb, nsh_len)))
  95                goto out;
  96
  97        proto = tun_p_to_eth_p(nsh_hdr(skb)->np);
  98        if (!proto)
  99                goto out;
 100
 101        __skb_pull(skb, nsh_len);
 102
 103        skb_reset_mac_header(skb);
 104        skb->mac_len = proto == htons(ETH_P_TEB) ? ETH_HLEN : 0;
 105        skb->protocol = proto;
 106
 107        features &= NETIF_F_SG;
 108        segs = skb_mac_gso_segment(skb, features);
 109        if (IS_ERR_OR_NULL(segs)) {
 110                skb_gso_error_unwind(skb, htons(ETH_P_NSH), nsh_len,
 111                                     skb->network_header - nhoff,
 112                                     mac_len);
 113                goto out;
 114        }
 115
 116        for (skb = segs; skb; skb = skb->next) {
 117                skb->protocol = htons(ETH_P_NSH);
 118                __skb_push(skb, nsh_len);
 119                skb_set_mac_header(skb, -nhoff);
 120                skb->network_header = skb->mac_header + mac_len;
 121                skb->mac_len = mac_len;
 122        }
 123
 124out:
 125        return segs;
 126}
 127
 128static struct packet_offload nsh_packet_offload __read_mostly = {
 129        .type = htons(ETH_P_NSH),
 130        .priority = 15,
 131        .callbacks = {
 132                .gso_segment = nsh_gso_segment,
 133        },
 134};
 135
 136static int __init nsh_init_module(void)
 137{
 138        dev_add_offload(&nsh_packet_offload);
 139        return 0;
 140}
 141
 142static void __exit nsh_cleanup_module(void)
 143{
 144        dev_remove_offload(&nsh_packet_offload);
 145}
 146
 147module_init(nsh_init_module);
 148module_exit(nsh_cleanup_module);
 149
 150MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
 151MODULE_DESCRIPTION("NSH protocol");
 152MODULE_LICENSE("GPL v2");
 153