// SPDX-License-Identifier: GPL-2.0-only
/*
 * Network Service Header
 *
 * Copyright (c) 2017 Red Hat, Inc. -- Jiri Benc <jbenc@redhat.com>
 */

#include <linux/module.h>
#include <linux/netdevice.h>
#include <linux/skbuff.h>
#include <net/gso.h>
#include <net/nsh.h>
#include <net/tun_proto.h>

int nsh_push(struct sk_buff *skb, const struct nshhdr *pushed_nh)
{
        struct nshhdr *nh;
        size_t length = nsh_hdr_len(pushed_nh);
        u8 next_proto;

        if (skb->mac_len) {
                next_proto = TUN_P_ETHERNET;
        } else {
                next_proto = tun_p_from_eth_p(skb->protocol);
                if (!next_proto)
                        return -EAFNOSUPPORT;
        }

        /* Add the NSH header */
        if (skb_cow_head(skb, length) < 0)
                return -ENOMEM;

        skb_push(skb, length);
        nh = (struct nshhdr *)(skb->data);
        memcpy(nh, pushed_nh, length);
        nh->np = next_proto;
        skb_postpush_rcsum(skb, nh, length);

        skb->protocol = htons(ETH_P_NSH);
        skb_reset_mac_header(skb);
        skb_reset_network_header(skb);
        skb_reset_mac_len(skb);

        return 0;
}
EXPORT_SYMBOL_GPL(nsh_push);

int nsh_pop(struct sk_buff *skb)
{
        struct nshhdr *nh;
        size_t length;
        __be16 inner_proto;

        if (!pskb_may_pull(skb, NSH_BASE_HDR_LEN))
                return -ENOMEM;
        nh = (struct nshhdr *)(skb->data);
        length = nsh_hdr_len(nh);
        if (length < NSH_BASE_HDR_LEN)
                return -EINVAL;
        inner_proto = tun_p_to_eth_p(nh->np);
        if (!pskb_may_pull(skb, length))
                return -ENOMEM;

        if (!inner_proto)
                return -EAFNOSUPPORT;

        skb_pull_rcsum(skb, length);
        skb_reset_mac_header(skb);
        skb_reset_network_header(skb);
        skb_reset_mac_len(skb);
        skb->protocol = inner_proto;

        return 0;
}
EXPORT_SYMBOL_GPL(nsh_pop);

static struct sk_buff *nsh_gso_segment(struct sk_buff *skb,
                                       netdev_features_t features)
{
        unsigned int outer_hlen, mac_len, nsh_len;
        struct sk_buff *segs = ERR_PTR(-EINVAL);
        u16 mac_offset = skb->mac_header;
        __be16 outer_proto, proto;

        skb_reset_network_header(skb);

        outer_proto = skb->protocol;
        outer_hlen = skb_mac_header_len(skb);
        mac_len = skb->mac_len;

        if (unlikely(!pskb_may_pull(skb, NSH_BASE_HDR_LEN)))
                goto out;
        nsh_len = nsh_hdr_len(nsh_hdr(skb));
        if (nsh_len < NSH_BASE_HDR_LEN)
                goto out;
        if (unlikely(!pskb_may_pull(skb, nsh_len)))
                goto out;

        proto = tun_p_to_eth_p(nsh_hdr(skb)->np);
        if (!proto)
                goto out;

        __skb_pull(skb, nsh_len);

        skb_reset_mac_header(skb);
        skb->mac_len = proto == htons(ETH_P_TEB) ? ETH_HLEN : 0;
        skb->protocol = proto;

        features &= NETIF_F_SG;
        segs = skb_mac_gso_segment(skb, features);
        if (IS_ERR_OR_NULL(segs)) {
                skb_gso_error_unwind(skb, htons(ETH_P_NSH), nsh_len,
                                     mac_offset, mac_len);
                goto out;
        }

        for (skb = segs; skb; skb = skb->next) {
                skb->protocol = outer_proto;
                __skb_push(skb, nsh_len + outer_hlen);
                skb_reset_mac_header(skb);
                skb_set_network_header(skb, outer_hlen);
                skb->mac_len = mac_len;
        }

out:
        return segs;
}

static struct packet_offload nsh_packet_offload __read_mostly = {
        .type = htons(ETH_P_NSH),
        .priority = 15,
        .callbacks = {
                .gso_segment = nsh_gso_segment,
        },
};

static int __init nsh_init_module(void)
{
        dev_add_offload(&nsh_packet_offload);
        return 0;
}

static void __exit nsh_cleanup_module(void)
{
        dev_remove_offload(&nsh_packet_offload);
}

module_init(nsh_init_module);
module_exit(nsh_cleanup_module);

MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
MODULE_DESCRIPTION("NSH protocol");
MODULE_LICENSE("GPL v2");