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