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	struct sk_buff *segs = ERR_PTR(-EINVAL);
81	u16 mac_offset = skb->mac_header;
82	unsigned int nsh_len, mac_len;
83	__be16 proto;
84
85	skb_reset_network_header(skb);
86
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				     mac_offset, mac_len);
112		goto out;
113	}
114
115	for (skb = segs; skb; skb = skb->next) {
116		skb->protocol = htons(ETH_P_NSH);
117		__skb_push(skb, nsh_len);
118		skb->mac_header = mac_offset;
119		skb->network_header = skb->mac_header + mac_len;
120		skb->mac_len = mac_len;
121	}
122
123out:
124	return segs;
125}
126
127static struct packet_offload nsh_packet_offload __read_mostly = {
128	.type = htons(ETH_P_NSH),
129	.priority = 15,
130	.callbacks = {
131		.gso_segment = nsh_gso_segment,
132	},
133};
134
135static int __init nsh_init_module(void)
136{
137	dev_add_offload(&nsh_packet_offload);
138	return 0;
139}
140
141static void __exit nsh_cleanup_module(void)
142{
143	dev_remove_offload(&nsh_packet_offload);
144}
145
146module_init(nsh_init_module);
147module_exit(nsh_cleanup_module);
148
149MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
150MODULE_DESCRIPTION("NSH protocol");
151MODULE_LICENSE("GPL v2");
152