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
 17int 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}
 48EXPORT_SYMBOL_GPL(nsh_push);
 49
 50int 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}
 77EXPORT_SYMBOL_GPL(nsh_pop);
 78
 79static 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_reset_mac_len(skb);
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
127out:
128	return segs;
129}
130
131static 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
139static int __init nsh_init_module(void)
140{
141	dev_add_offload(&nsh_packet_offload);
142	return 0;
143}
144
145static void __exit nsh_cleanup_module(void)
146{
147	dev_remove_offload(&nsh_packet_offload);
148}
149
150module_init(nsh_init_module);
151module_exit(nsh_cleanup_module);
152
153MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
154MODULE_DESCRIPTION("NSH protocol");
155MODULE_LICENSE("GPL v2");