1// SPDX-License-Identifier: GPL-2.0-or-later
2/* xfrm6_protocol.c - Generic xfrm protocol multiplexer for ipv6.
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/xfrm4_protocol.c
11 */
12
13#include <linux/init.h>
14#include <linux/mutex.h>
15#include <linux/skbuff.h>
16#include <linux/icmpv6.h>
17#include <net/ip6_route.h>
18#include <net/ipv6.h>
19#include <net/protocol.h>
20#include <net/xfrm.h>
21
22static struct xfrm6_protocol __rcu *esp6_handlers __read_mostly;
23static struct xfrm6_protocol __rcu *ah6_handlers __read_mostly;
24static struct xfrm6_protocol __rcu *ipcomp6_handlers __read_mostly;
25static DEFINE_MUTEX(xfrm6_protocol_mutex);
26
27static inline struct xfrm6_protocol __rcu **proto_handlers(u8 protocol)
28{
29	switch (protocol) {
30	case IPPROTO_ESP:
31		return &esp6_handlers;
32	case IPPROTO_AH:
33		return &ah6_handlers;
34	case IPPROTO_COMP:
35		return &ipcomp6_handlers;
36	}
37
38	return NULL;
39}
40
41#define for_each_protocol_rcu(head, handler)		\
42	for (handler = rcu_dereference(head);		\
43	     handler != NULL;				\
44	     handler = rcu_dereference(handler->next))	\
45
46static int xfrm6_rcv_cb(struct sk_buff *skb, u8 protocol, int err)
47{
48	int ret;
49	struct xfrm6_protocol *handler;
50	struct xfrm6_protocol __rcu **head = proto_handlers(protocol);
51
52	if (!head)
53		return 0;
54
55	for_each_protocol_rcu(*proto_handlers(protocol), handler)
56		if ((ret = handler->cb_handler(skb, err)) <= 0)
57			return ret;
58
59	return 0;
60}
61
62int xfrm6_rcv_encap(struct sk_buff *skb, int nexthdr, __be32 spi,
63		    int encap_type)
64{
65	int ret;
66	struct xfrm6_protocol *handler;
67	struct xfrm6_protocol __rcu **head = proto_handlers(nexthdr);
68
69	XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip6 = NULL;
70	XFRM_SPI_SKB_CB(skb)->family = AF_INET6;
71	XFRM_SPI_SKB_CB(skb)->daddroff = offsetof(struct ipv6hdr, daddr);
72
73	if (!head)
74		goto out;
75
76	if (!skb_dst(skb)) {
77		const struct ipv6hdr *ip6h = ipv6_hdr(skb);
78		int flags = RT6_LOOKUP_F_HAS_SADDR;
79		struct dst_entry *dst;
80		struct flowi6 fl6 = {
81			.flowi6_iif   = skb->dev->ifindex,
82			.daddr        = ip6h->daddr,
83			.saddr        = ip6h->saddr,
84			.flowlabel    = ip6_flowinfo(ip6h),
85			.flowi6_mark  = skb->mark,
86			.flowi6_proto = ip6h->nexthdr,
87		};
88
89		dst = ip6_route_input_lookup(dev_net(skb->dev), skb->dev, &fl6,
90					     skb, flags);
91		if (dst->error)
92			goto drop;
93		skb_dst_set(skb, dst);
94	}
95
96	for_each_protocol_rcu(*head, handler)
97		if ((ret = handler->input_handler(skb, nexthdr, spi, encap_type)) != -EINVAL)
98			return ret;
99
100out:
101	icmpv6_send(skb, ICMPV6_DEST_UNREACH, ICMPV6_PORT_UNREACH, 0);
102
103drop:
104	kfree_skb(skb);
105	return 0;
106}
107EXPORT_SYMBOL(xfrm6_rcv_encap);
108
109static int xfrm6_esp_rcv(struct sk_buff *skb)
110{
111	int ret;
112	struct xfrm6_protocol *handler;
113
114	XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip6 = NULL;
115
116	for_each_protocol_rcu(esp6_handlers, handler)
117		if ((ret = handler->handler(skb)) != -EINVAL)
118			return ret;
119
120	icmpv6_send(skb, ICMPV6_DEST_UNREACH, ICMPV6_PORT_UNREACH, 0);
121
122	kfree_skb(skb);
123	return 0;
124}
125
126static int xfrm6_esp_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
127			  u8 type, u8 code, int offset, __be32 info)
128{
129	struct xfrm6_protocol *handler;
130
131	for_each_protocol_rcu(esp6_handlers, handler)
132		if (!handler->err_handler(skb, opt, type, code, offset, info))
133			return 0;
134
135	return -ENOENT;
136}
137
138static int xfrm6_ah_rcv(struct sk_buff *skb)
139{
140	int ret;
141	struct xfrm6_protocol *handler;
142
143	XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip6 = NULL;
144
145	for_each_protocol_rcu(ah6_handlers, handler)
146		if ((ret = handler->handler(skb)) != -EINVAL)
147			return ret;
148
149	icmpv6_send(skb, ICMPV6_DEST_UNREACH, ICMPV6_PORT_UNREACH, 0);
150
151	kfree_skb(skb);
152	return 0;
153}
154
155static int xfrm6_ah_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
156			 u8 type, u8 code, int offset, __be32 info)
157{
158	struct xfrm6_protocol *handler;
159
160	for_each_protocol_rcu(ah6_handlers, handler)
161		if (!handler->err_handler(skb, opt, type, code, offset, info))
162			return 0;
163
164	return -ENOENT;
165}
166
167static int xfrm6_ipcomp_rcv(struct sk_buff *skb)
168{
169	int ret;
170	struct xfrm6_protocol *handler;
171
172	XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip6 = NULL;
173
174	for_each_protocol_rcu(ipcomp6_handlers, handler)
175		if ((ret = handler->handler(skb)) != -EINVAL)
176			return ret;
177
178	icmpv6_send(skb, ICMPV6_DEST_UNREACH, ICMPV6_PORT_UNREACH, 0);
179
180	kfree_skb(skb);
181	return 0;
182}
183
184static int xfrm6_ipcomp_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
185			     u8 type, u8 code, int offset, __be32 info)
186{
187	struct xfrm6_protocol *handler;
188
189	for_each_protocol_rcu(ipcomp6_handlers, handler)
190		if (!handler->err_handler(skb, opt, type, code, offset, info))
191			return 0;
192
193	return -ENOENT;
194}
195
196static const struct inet6_protocol esp6_protocol = {
197	.handler	=	xfrm6_esp_rcv,
198	.err_handler	=	xfrm6_esp_err,
199	.flags		=	INET6_PROTO_NOPOLICY,
200};
201
202static const struct inet6_protocol ah6_protocol = {
203	.handler	=	xfrm6_ah_rcv,
204	.err_handler	=	xfrm6_ah_err,
205	.flags		=	INET6_PROTO_NOPOLICY,
206};
207
208static const struct inet6_protocol ipcomp6_protocol = {
209	.handler	=	xfrm6_ipcomp_rcv,
210	.err_handler	=	xfrm6_ipcomp_err,
211	.flags		=	INET6_PROTO_NOPOLICY,
212};
213
214static const struct xfrm_input_afinfo xfrm6_input_afinfo = {
215	.family		=	AF_INET6,
216	.callback	=	xfrm6_rcv_cb,
217};
218
219static inline const struct inet6_protocol *netproto(unsigned char protocol)
220{
221	switch (protocol) {
222	case IPPROTO_ESP:
223		return &esp6_protocol;
224	case IPPROTO_AH:
225		return &ah6_protocol;
226	case IPPROTO_COMP:
227		return &ipcomp6_protocol;
228	}
229
230	return NULL;
231}
232
233int xfrm6_protocol_register(struct xfrm6_protocol *handler,
234			    unsigned char protocol)
235{
236	struct xfrm6_protocol __rcu **pprev;
237	struct xfrm6_protocol *t;
238	bool add_netproto = false;
239	int ret = -EEXIST;
240	int priority = handler->priority;
241
242	if (!proto_handlers(protocol) || !netproto(protocol))
243		return -EINVAL;
244
245	mutex_lock(&xfrm6_protocol_mutex);
246
247	if (!rcu_dereference_protected(*proto_handlers(protocol),
248				       lockdep_is_held(&xfrm6_protocol_mutex)))
249		add_netproto = true;
250
251	for (pprev = proto_handlers(protocol);
252	     (t = rcu_dereference_protected(*pprev,
253			lockdep_is_held(&xfrm6_protocol_mutex))) != NULL;
254	     pprev = &t->next) {
255		if (t->priority < priority)
256			break;
257		if (t->priority == priority)
258			goto err;
259	}
260
261	handler->next = *pprev;
262	rcu_assign_pointer(*pprev, handler);
263
264	ret = 0;
265
266err:
267	mutex_unlock(&xfrm6_protocol_mutex);
268
269	if (add_netproto) {
270		if (inet6_add_protocol(netproto(protocol), protocol)) {
271			pr_err("%s: can't add protocol\n", __func__);
272			ret = -EAGAIN;
273		}
274	}
275
276	return ret;
277}
278EXPORT_SYMBOL(xfrm6_protocol_register);
279
280int xfrm6_protocol_deregister(struct xfrm6_protocol *handler,
281			      unsigned char protocol)
282{
283	struct xfrm6_protocol __rcu **pprev;
284	struct xfrm6_protocol *t;
285	int ret = -ENOENT;
286
287	if (!proto_handlers(protocol) || !netproto(protocol))
288		return -EINVAL;
289
290	mutex_lock(&xfrm6_protocol_mutex);
291
292	for (pprev = proto_handlers(protocol);
293	     (t = rcu_dereference_protected(*pprev,
294			lockdep_is_held(&xfrm6_protocol_mutex))) != NULL;
295	     pprev = &t->next) {
296		if (t == handler) {
297			*pprev = handler->next;
298			ret = 0;
299			break;
300		}
301	}
302
303	if (!rcu_dereference_protected(*proto_handlers(protocol),
304				       lockdep_is_held(&xfrm6_protocol_mutex))) {
305		if (inet6_del_protocol(netproto(protocol), protocol) < 0) {
306			pr_err("%s: can't remove protocol\n", __func__);
307			ret = -EAGAIN;
308		}
309	}
310
311	mutex_unlock(&xfrm6_protocol_mutex);
312
313	synchronize_net();
314
315	return ret;
316}
317EXPORT_SYMBOL(xfrm6_protocol_deregister);
318
319int __init xfrm6_protocol_init(void)
320{
321	return xfrm_input_register_afinfo(&xfrm6_input_afinfo);
322}
323
324void xfrm6_protocol_fini(void)
325{
326	xfrm_input_unregister_afinfo(&xfrm6_input_afinfo);
327}
328