1// SPDX-License-Identifier: GPL-2.0-or-later
2/* xfrm4_protocol.c - Generic xfrm protocol multiplexer.
3 *
4 * Copyright (C) 2013 secunet Security Networks AG
5 *
6 * Author:
7 * Steffen Klassert <steffen.klassert@secunet.com>
8 *
9 * Based on:
10 * net/ipv4/tunnel4.c
11 */
12
13#include <linux/init.h>
14#include <linux/mutex.h>
15#include <linux/skbuff.h>
16#include <net/icmp.h>
17#include <net/ip.h>
18#include <net/protocol.h>
19#include <net/xfrm.h>
20
21static struct xfrm4_protocol __rcu *esp4_handlers __read_mostly;
22static struct xfrm4_protocol __rcu *ah4_handlers __read_mostly;
23static struct xfrm4_protocol __rcu *ipcomp4_handlers __read_mostly;
24static DEFINE_MUTEX(xfrm4_protocol_mutex);
25
26static inline struct xfrm4_protocol __rcu **proto_handlers(u8 protocol)
27{
28	switch (protocol) {
29	case IPPROTO_ESP:
30		return &esp4_handlers;
31	case IPPROTO_AH:
32		return &ah4_handlers;
33	case IPPROTO_COMP:
34		return &ipcomp4_handlers;
35	}
36
37	return NULL;
38}
39
40#define for_each_protocol_rcu(head, handler)		\
41	for (handler = rcu_dereference(head);		\
42	     handler != NULL;				\
43	     handler = rcu_dereference(handler->next))	\
44
45static int xfrm4_rcv_cb(struct sk_buff *skb, u8 protocol, int err)
46{
47	int ret;
48	struct xfrm4_protocol *handler;
49	struct xfrm4_protocol __rcu **head = proto_handlers(protocol);
50
51	if (!head)
52		return 0;
53
54	for_each_protocol_rcu(*head, handler)
55		if ((ret = handler->cb_handler(skb, err)) <= 0)
56			return ret;
57
58	return 0;
59}
60
61int xfrm4_rcv_encap(struct sk_buff *skb, int nexthdr, __be32 spi,
62		    int encap_type)
63{
64	int ret;
65	struct xfrm4_protocol *handler;
66	struct xfrm4_protocol __rcu **head = proto_handlers(nexthdr);
67
68	XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip4 = NULL;
69	XFRM_SPI_SKB_CB(skb)->family = AF_INET;
70	XFRM_SPI_SKB_CB(skb)->daddroff = offsetof(struct iphdr, daddr);
71
72	if (!head)
73		goto out;
74
75	if (!skb_dst(skb)) {
76		const struct iphdr *iph = ip_hdr(skb);
77
78		if (ip_route_input_noref(skb, iph->daddr, iph->saddr,
79					 iph->tos, skb->dev))
80			goto drop;
81	}
82
83	for_each_protocol_rcu(*head, handler)
84		if ((ret = handler->input_handler(skb, nexthdr, spi, encap_type)) != -EINVAL)
85			return ret;
86
87out:
88	icmp_send(skb, ICMP_DEST_UNREACH, ICMP_PORT_UNREACH, 0);
89
90drop:
91	kfree_skb(skb);
92	return 0;
93}
94EXPORT_SYMBOL(xfrm4_rcv_encap);
95
96static int xfrm4_esp_rcv(struct sk_buff *skb)
97{
98	int ret;
99	struct xfrm4_protocol *handler;
100
101	XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip4 = NULL;
102
103	for_each_protocol_rcu(esp4_handlers, handler)
104		if ((ret = handler->handler(skb)) != -EINVAL)
105			return ret;
106
107	icmp_send(skb, ICMP_DEST_UNREACH, ICMP_PORT_UNREACH, 0);
108
109	kfree_skb(skb);
110	return 0;
111}
112
113static int xfrm4_esp_err(struct sk_buff *skb, u32 info)
114{
115	struct xfrm4_protocol *handler;
116
117	for_each_protocol_rcu(esp4_handlers, handler)
118		if (!handler->err_handler(skb, info))
119			return 0;
120
121	return -ENOENT;
122}
123
124static int xfrm4_ah_rcv(struct sk_buff *skb)
125{
126	int ret;
127	struct xfrm4_protocol *handler;
128
129	XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip4 = NULL;
130
131	for_each_protocol_rcu(ah4_handlers, handler)
132		if ((ret = handler->handler(skb)) != -EINVAL)
133			return ret;
134
135	icmp_send(skb, ICMP_DEST_UNREACH, ICMP_PORT_UNREACH, 0);
136
137	kfree_skb(skb);
138	return 0;
139}
140
141static int xfrm4_ah_err(struct sk_buff *skb, u32 info)
142{
143	struct xfrm4_protocol *handler;
144
145	for_each_protocol_rcu(ah4_handlers, handler)
146		if (!handler->err_handler(skb, info))
147			return 0;
148
149	return -ENOENT;
150}
151
152static int xfrm4_ipcomp_rcv(struct sk_buff *skb)
153{
154	int ret;
155	struct xfrm4_protocol *handler;
156
157	XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip4 = NULL;
158
159	for_each_protocol_rcu(ipcomp4_handlers, handler)
160		if ((ret = handler->handler(skb)) != -EINVAL)
161			return ret;
162
163	icmp_send(skb, ICMP_DEST_UNREACH, ICMP_PORT_UNREACH, 0);
164
165	kfree_skb(skb);
166	return 0;
167}
168
169static int xfrm4_ipcomp_err(struct sk_buff *skb, u32 info)
170{
171	struct xfrm4_protocol *handler;
172
173	for_each_protocol_rcu(ipcomp4_handlers, handler)
174		if (!handler->err_handler(skb, info))
175			return 0;
176
177	return -ENOENT;
178}
179
180static const struct net_protocol esp4_protocol = {
181	.handler	=	xfrm4_esp_rcv,
182	.err_handler	=	xfrm4_esp_err,
183	.no_policy	=	1,
184};
185
186static const struct net_protocol ah4_protocol = {
187	.handler	=	xfrm4_ah_rcv,
188	.err_handler	=	xfrm4_ah_err,
189	.no_policy	=	1,
190};
191
192static const struct net_protocol ipcomp4_protocol = {
193	.handler	=	xfrm4_ipcomp_rcv,
194	.err_handler	=	xfrm4_ipcomp_err,
195	.no_policy	=	1,
196};
197
198static const struct xfrm_input_afinfo xfrm4_input_afinfo = {
199	.family		=	AF_INET,
200	.callback	=	xfrm4_rcv_cb,
201};
202
203static inline const struct net_protocol *netproto(unsigned char protocol)
204{
205	switch (protocol) {
206	case IPPROTO_ESP:
207		return &esp4_protocol;
208	case IPPROTO_AH:
209		return &ah4_protocol;
210	case IPPROTO_COMP:
211		return &ipcomp4_protocol;
212	}
213
214	return NULL;
215}
216
217int xfrm4_protocol_register(struct xfrm4_protocol *handler,
218			    unsigned char protocol)
219{
220	struct xfrm4_protocol __rcu **pprev;
221	struct xfrm4_protocol *t;
222	bool add_netproto = false;
223	int ret = -EEXIST;
224	int priority = handler->priority;
225
226	if (!proto_handlers(protocol) || !netproto(protocol))
227		return -EINVAL;
228
229	mutex_lock(&xfrm4_protocol_mutex);
230
231	if (!rcu_dereference_protected(*proto_handlers(protocol),
232				       lockdep_is_held(&xfrm4_protocol_mutex)))
233		add_netproto = true;
234
235	for (pprev = proto_handlers(protocol);
236	     (t = rcu_dereference_protected(*pprev,
237			lockdep_is_held(&xfrm4_protocol_mutex))) != NULL;
238	     pprev = &t->next) {
239		if (t->priority < priority)
240			break;
241		if (t->priority == priority)
242			goto err;
243	}
244
245	handler->next = *pprev;
246	rcu_assign_pointer(*pprev, handler);
247
248	ret = 0;
249
250err:
251	mutex_unlock(&xfrm4_protocol_mutex);
252
253	if (add_netproto) {
254		if (inet_add_protocol(netproto(protocol), protocol)) {
255			pr_err("%s: can't add protocol\n", __func__);
256			ret = -EAGAIN;
257		}
258	}
259
260	return ret;
261}
262EXPORT_SYMBOL(xfrm4_protocol_register);
263
264int xfrm4_protocol_deregister(struct xfrm4_protocol *handler,
265			      unsigned char protocol)
266{
267	struct xfrm4_protocol __rcu **pprev;
268	struct xfrm4_protocol *t;
269	int ret = -ENOENT;
270
271	if (!proto_handlers(protocol) || !netproto(protocol))
272		return -EINVAL;
273
274	mutex_lock(&xfrm4_protocol_mutex);
275
276	for (pprev = proto_handlers(protocol);
277	     (t = rcu_dereference_protected(*pprev,
278			lockdep_is_held(&xfrm4_protocol_mutex))) != NULL;
279	     pprev = &t->next) {
280		if (t == handler) {
281			*pprev = handler->next;
282			ret = 0;
283			break;
284		}
285	}
286
287	if (!rcu_dereference_protected(*proto_handlers(protocol),
288				       lockdep_is_held(&xfrm4_protocol_mutex))) {
289		if (inet_del_protocol(netproto(protocol), protocol) < 0) {
290			pr_err("%s: can't remove protocol\n", __func__);
291			ret = -EAGAIN;
292		}
293	}
294
295	mutex_unlock(&xfrm4_protocol_mutex);
296
297	synchronize_net();
298
299	return ret;
300}
301EXPORT_SYMBOL(xfrm4_protocol_deregister);
302
303void __init xfrm4_protocol_init(void)
304{
305	xfrm_input_register_afinfo(&xfrm4_input_afinfo);
306}
307