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 
nsh_push(struct sk_buff * skb,const struct nshhdr * pushed_nh)14 int 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 }
45 EXPORT_SYMBOL_GPL(nsh_push);
46 
nsh_pop(struct sk_buff * skb)47 int 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 }
74 EXPORT_SYMBOL_GPL(nsh_pop);
75 
nsh_gso_segment(struct sk_buff * skb,netdev_features_t features)76 static 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 
124 out:
125 	return segs;
126 }
127 
128 static 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 
nsh_init_module(void)136 static int __init nsh_init_module(void)
137 {
138 	dev_add_offload(&nsh_packet_offload);
139 	return 0;
140 }
141 
nsh_cleanup_module(void)142 static void __exit nsh_cleanup_module(void)
143 {
144 	dev_remove_offload(&nsh_packet_offload);
145 }
146 
147 module_init(nsh_init_module);
148 module_exit(nsh_cleanup_module);
149 
150 MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
151 MODULE_DESCRIPTION("NSH protocol");
152 MODULE_LICENSE("GPL v2");
153