allowedips.c

  1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * Copyright (C) 2015-2019 Jason A. Donenfeld <Jason@zx2c4.com>. All Rights Reserved.
  4 */
  5
  6#include "allowedips.h"
  7#include "peer.h"
  8
  9enum { MAX_ALLOWEDIPS_DEPTH = 129 };
 10
 11static struct kmem_cache *node_cache;
 12
 13static void swap_endian(u8 *dst, const u8 *src, u8 bits)
 14{
 15	if (bits == 32) {
 16		*(u32 *)dst = be32_to_cpu(*(const __be32 *)src);
 17	} else if (bits == 128) {
 18		((u64 *)dst)[0] = get_unaligned_be64(src);
 19		((u64 *)dst)[1] = get_unaligned_be64(src + 8);
 20	}
 21}
 22
 23static void copy_and_assign_cidr(struct allowedips_node *node, const u8 *src,
 24				 u8 cidr, u8 bits)
 25{
 26	node->cidr = cidr;
 27	node->bit_at_a = cidr / 8U;
 28#ifdef __LITTLE_ENDIAN
 29	node->bit_at_a ^= (bits / 8U - 1U) % 8U;
 30#endif
 31	node->bit_at_b = 7U - (cidr % 8U);
 32	node->bitlen = bits;
 33	memcpy(node->bits, src, bits / 8U);
 34}
 35
 36static inline u8 choose(struct allowedips_node *node, const u8 *key)
 37{
 38	return (key[node->bit_at_a] >> node->bit_at_b) & 1;
 39}
 40
 41static void push_rcu(struct allowedips_node **stack,
 42		     struct allowedips_node __rcu *p, unsigned int *len)
 43{
 44	if (rcu_access_pointer(p)) {
 45		if (WARN_ON(IS_ENABLED(DEBUG) && *len >= MAX_ALLOWEDIPS_DEPTH))
 46			return;
 47		stack[(*len)++] = rcu_dereference_raw(p);
 48	}
 49}
 50
 51static void node_free_rcu(struct rcu_head *rcu)
 52{
 53	kmem_cache_free(node_cache, container_of(rcu, struct allowedips_node, rcu));
 54}
 55
 56static void root_free_rcu(struct rcu_head *rcu)
 57{
 58	struct allowedips_node *node, *stack[MAX_ALLOWEDIPS_DEPTH] = {
 59		container_of(rcu, struct allowedips_node, rcu) };
 60	unsigned int len = 1;
 61
 62	while (len > 0 && (node = stack[--len])) {
 63		push_rcu(stack, node->bit[0], &len);
 64		push_rcu(stack, node->bit[1], &len);
 65		kmem_cache_free(node_cache, node);
 66	}
 67}
 68
 69static void root_remove_peer_lists(struct allowedips_node *root)
 70{
 71	struct allowedips_node *node, *stack[MAX_ALLOWEDIPS_DEPTH] = { root };
 72	unsigned int len = 1;
 73
 74	while (len > 0 && (node = stack[--len])) {
 75		push_rcu(stack, node->bit[0], &len);
 76		push_rcu(stack, node->bit[1], &len);
 77		if (rcu_access_pointer(node->peer))
 78			list_del(&node->peer_list);
 79	}
 80}
 81
 82static unsigned int fls128(u64 a, u64 b)
 83{
 84	return a ? fls64(a) + 64U : fls64(b);
 85}
 86
 87static u8 common_bits(const struct allowedips_node *node, const u8 *key,
 88		      u8 bits)
 89{
 90	if (bits == 32)
 91		return 32U - fls(*(const u32 *)node->bits ^ *(const u32 *)key);
 92	else if (bits == 128)
 93		return 128U - fls128(
 94			*(const u64 *)&node->bits[0] ^ *(const u64 *)&key[0],
 95			*(const u64 *)&node->bits[8] ^ *(const u64 *)&key[8]);
 96	return 0;
 97}
 98
 99static bool prefix_matches(const struct allowedips_node *node, const u8 *key,
100			   u8 bits)
101{
102	/* This could be much faster if it actually just compared the common
103	 * bits properly, by precomputing a mask bswap(~0 << (32 - cidr)), and
104	 * the rest, but it turns out that common_bits is already super fast on
105	 * modern processors, even taking into account the unfortunate bswap.
106	 * So, we just inline it like this instead.
107	 */
108	return common_bits(node, key, bits) >= node->cidr;
109}
110
111static struct allowedips_node *find_node(struct allowedips_node *trie, u8 bits,
112					 const u8 *key)
113{
114	struct allowedips_node *node = trie, *found = NULL;
115
116	while (node && prefix_matches(node, key, bits)) {
117		if (rcu_access_pointer(node->peer))
118			found = node;
119		if (node->cidr == bits)
120			break;
121		node = rcu_dereference_bh(node->bit[choose(node, key)]);
122	}
123	return found;
124}
125
126/* Returns a strong reference to a peer */
127static struct wg_peer *lookup(struct allowedips_node __rcu *root, u8 bits,
128			      const void *be_ip)
129{
130	/* Aligned so it can be passed to fls/fls64 */
131	u8 ip[16] __aligned(__alignof(u64));
132	struct allowedips_node *node;
133	struct wg_peer *peer = NULL;
134
135	swap_endian(ip, be_ip, bits);
136
137	rcu_read_lock_bh();
138retry:
139	node = find_node(rcu_dereference_bh(root), bits, ip);
140	if (node) {
141		peer = wg_peer_get_maybe_zero(rcu_dereference_bh(node->peer));
142		if (!peer)
143			goto retry;
144	}
145	rcu_read_unlock_bh();
146	return peer;
147}
148
149static bool node_placement(struct allowedips_node __rcu *trie, const u8 *key,
150			   u8 cidr, u8 bits, struct allowedips_node **rnode,
151			   struct mutex *lock)
152{
153	struct allowedips_node *node = rcu_dereference_protected(trie, lockdep_is_held(lock));
154	struct allowedips_node *parent = NULL;
155	bool exact = false;
156
157	while (node && node->cidr <= cidr && prefix_matches(node, key, bits)) {
158		parent = node;
159		if (parent->cidr == cidr) {
160			exact = true;
161			break;
162		}
163		node = rcu_dereference_protected(parent->bit[choose(parent, key)], lockdep_is_held(lock));
164	}
165	*rnode = parent;
166	return exact;
167}
168
169static inline void connect_node(struct allowedips_node __rcu **parent, u8 bit, struct allowedips_node *node)
170{
171	node->parent_bit_packed = (unsigned long)parent | bit;
172	rcu_assign_pointer(*parent, node);
173}
174
175static inline void choose_and_connect_node(struct allowedips_node *parent, struct allowedips_node *node)
176{
177	u8 bit = choose(parent, node->bits);
178	connect_node(&parent->bit[bit], bit, node);
179}
180
181static int add(struct allowedips_node __rcu **trie, u8 bits, const u8 *key,
182	       u8 cidr, struct wg_peer *peer, struct mutex *lock)
183{
184	struct allowedips_node *node, *parent, *down, *newnode;
185
186	if (unlikely(cidr > bits || !peer))
187		return -EINVAL;
188
189	if (!rcu_access_pointer(*trie)) {
190		node = kmem_cache_zalloc(node_cache, GFP_KERNEL);
191		if (unlikely(!node))
192			return -ENOMEM;
193		RCU_INIT_POINTER(node->peer, peer);
194		list_add_tail(&node->peer_list, &peer->allowedips_list);
195		copy_and_assign_cidr(node, key, cidr, bits);
196		connect_node(trie, 2, node);
197		return 0;
198	}
199	if (node_placement(*trie, key, cidr, bits, &node, lock)) {
200		rcu_assign_pointer(node->peer, peer);
201		list_move_tail(&node->peer_list, &peer->allowedips_list);
202		return 0;
203	}
204
205	newnode = kmem_cache_zalloc(node_cache, GFP_KERNEL);
206	if (unlikely(!newnode))
207		return -ENOMEM;
208	RCU_INIT_POINTER(newnode->peer, peer);
209	list_add_tail(&newnode->peer_list, &peer->allowedips_list);
210	copy_and_assign_cidr(newnode, key, cidr, bits);
211
212	if (!node) {
213		down = rcu_dereference_protected(*trie, lockdep_is_held(lock));
214	} else {
215		const u8 bit = choose(node, key);
216		down = rcu_dereference_protected(node->bit[bit], lockdep_is_held(lock));
217		if (!down) {
218			connect_node(&node->bit[bit], bit, newnode);
219			return 0;
220		}
221	}
222	cidr = min(cidr, common_bits(down, key, bits));
223	parent = node;
224
225	if (newnode->cidr == cidr) {
226		choose_and_connect_node(newnode, down);
227		if (!parent)
228			connect_node(trie, 2, newnode);
229		else
230			choose_and_connect_node(parent, newnode);
231		return 0;
232	}
233
234	node = kmem_cache_zalloc(node_cache, GFP_KERNEL);
235	if (unlikely(!node)) {
236		list_del(&newnode->peer_list);
237		kmem_cache_free(node_cache, newnode);
238		return -ENOMEM;
239	}
240	INIT_LIST_HEAD(&node->peer_list);
241	copy_and_assign_cidr(node, newnode->bits, cidr, bits);
242
243	choose_and_connect_node(node, down);
244	choose_and_connect_node(node, newnode);
245	if (!parent)
246		connect_node(trie, 2, node);
247	else
248		choose_and_connect_node(parent, node);
249	return 0;
250}
251
252static void remove_node(struct allowedips_node *node, struct mutex *lock)
253{
254	struct allowedips_node *child, **parent_bit, *parent;
255	bool free_parent;
256
257	list_del_init(&node->peer_list);
258	RCU_INIT_POINTER(node->peer, NULL);
259	if (node->bit[0] && node->bit[1])
260		return;
261	child = rcu_dereference_protected(node->bit[!rcu_access_pointer(node->bit[0])],
262					  lockdep_is_held(lock));
263	if (child)
264		child->parent_bit_packed = node->parent_bit_packed;
265	parent_bit = (struct allowedips_node **)(node->parent_bit_packed & ~3UL);
266	*parent_bit = child;
267	parent = (void *)parent_bit -
268			offsetof(struct allowedips_node, bit[node->parent_bit_packed & 1]);
269	free_parent = !rcu_access_pointer(node->bit[0]) && !rcu_access_pointer(node->bit[1]) &&
270			(node->parent_bit_packed & 3) <= 1 && !rcu_access_pointer(parent->peer);
271	if (free_parent)
272		child = rcu_dereference_protected(parent->bit[!(node->parent_bit_packed & 1)],
273						  lockdep_is_held(lock));
274	call_rcu(&node->rcu, node_free_rcu);
275	if (!free_parent)
276		return;
277	if (child)
278		child->parent_bit_packed = parent->parent_bit_packed;
279	*(struct allowedips_node **)(parent->parent_bit_packed & ~3UL) = child;
280	call_rcu(&parent->rcu, node_free_rcu);
281}
282
283static int remove(struct allowedips_node __rcu **trie, u8 bits, const u8 *key,
284		  u8 cidr, struct wg_peer *peer, struct mutex *lock)
285{
286	struct allowedips_node *node;
287
288	if (unlikely(cidr > bits))
289		return -EINVAL;
290	if (!rcu_access_pointer(*trie) || !node_placement(*trie, key, cidr, bits, &node, lock) ||
291	    peer != rcu_access_pointer(node->peer))
292		return 0;
293
294	remove_node(node, lock);
295	return 0;
296}
297
298void wg_allowedips_init(struct allowedips *table)
299{
300	table->root4 = table->root6 = NULL;
301	table->seq = 1;
302}
303
304void wg_allowedips_free(struct allowedips *table, struct mutex *lock)
305{
306	struct allowedips_node __rcu *old4 = table->root4, *old6 = table->root6;
307
308	++table->seq;
309	RCU_INIT_POINTER(table->root4, NULL);
310	RCU_INIT_POINTER(table->root6, NULL);
311	if (rcu_access_pointer(old4)) {
312		struct allowedips_node *node = rcu_dereference_protected(old4,
313							lockdep_is_held(lock));
314
315		root_remove_peer_lists(node);
316		call_rcu(&node->rcu, root_free_rcu);
317	}
318	if (rcu_access_pointer(old6)) {
319		struct allowedips_node *node = rcu_dereference_protected(old6,
320							lockdep_is_held(lock));
321
322		root_remove_peer_lists(node);
323		call_rcu(&node->rcu, root_free_rcu);
324	}
325}
326
327int wg_allowedips_insert_v4(struct allowedips *table, const struct in_addr *ip,
328			    u8 cidr, struct wg_peer *peer, struct mutex *lock)
329{
330	/* Aligned so it can be passed to fls */
331	u8 key[4] __aligned(__alignof(u32));
332
333	++table->seq;
334	swap_endian(key, (const u8 *)ip, 32);
335	return add(&table->root4, 32, key, cidr, peer, lock);
336}
337
338int wg_allowedips_insert_v6(struct allowedips *table, const struct in6_addr *ip,
339			    u8 cidr, struct wg_peer *peer, struct mutex *lock)
340{
341	/* Aligned so it can be passed to fls64 */
342	u8 key[16] __aligned(__alignof(u64));
343
344	++table->seq;
345	swap_endian(key, (const u8 *)ip, 128);
346	return add(&table->root6, 128, key, cidr, peer, lock);
347}
348
349int wg_allowedips_remove_v4(struct allowedips *table, const struct in_addr *ip,
350			    u8 cidr, struct wg_peer *peer, struct mutex *lock)
351{
352	/* Aligned so it can be passed to fls */
353	u8 key[4] __aligned(__alignof(u32));
354
355	++table->seq;
356	swap_endian(key, (const u8 *)ip, 32);
357	return remove(&table->root4, 32, key, cidr, peer, lock);
358}
359
360int wg_allowedips_remove_v6(struct allowedips *table, const struct in6_addr *ip,
361			    u8 cidr, struct wg_peer *peer, struct mutex *lock)
362{
363	/* Aligned so it can be passed to fls64 */
364	u8 key[16] __aligned(__alignof(u64));
365
366	++table->seq;
367	swap_endian(key, (const u8 *)ip, 128);
368	return remove(&table->root6, 128, key, cidr, peer, lock);
369}
370
371void wg_allowedips_remove_by_peer(struct allowedips *table,
372				  struct wg_peer *peer, struct mutex *lock)
373{
374	struct allowedips_node *node, *tmp;
375
376	if (list_empty(&peer->allowedips_list))
377		return;
378	++table->seq;
379	list_for_each_entry_safe(node, tmp, &peer->allowedips_list, peer_list)
380		remove_node(node, lock);
381}
382
383int wg_allowedips_read_node(struct allowedips_node *node, u8 ip[16], u8 *cidr)
384{
385	const unsigned int cidr_bytes = DIV_ROUND_UP(node->cidr, 8U);
386	swap_endian(ip, node->bits, node->bitlen);
387	memset(ip + cidr_bytes, 0, node->bitlen / 8U - cidr_bytes);
388	if (node->cidr)
389		ip[cidr_bytes - 1U] &= ~0U << (-node->cidr % 8U);
390
391	*cidr = node->cidr;
392	return node->bitlen == 32 ? AF_INET : AF_INET6;
393}
394
395/* Returns a strong reference to a peer */
396struct wg_peer *wg_allowedips_lookup_dst(struct allowedips *table,
397					 struct sk_buff *skb)
398{
399	if (skb->protocol == htons(ETH_P_IP))
400		return lookup(table->root4, 32, &ip_hdr(skb)->daddr);
401	else if (skb->protocol == htons(ETH_P_IPV6))
402		return lookup(table->root6, 128, &ipv6_hdr(skb)->daddr);
403	return NULL;
404}
405
406/* Returns a strong reference to a peer */
407struct wg_peer *wg_allowedips_lookup_src(struct allowedips *table,
408					 struct sk_buff *skb)
409{
410	if (skb->protocol == htons(ETH_P_IP))
411		return lookup(table->root4, 32, &ip_hdr(skb)->saddr);
412	else if (skb->protocol == htons(ETH_P_IPV6))
413		return lookup(table->root6, 128, &ipv6_hdr(skb)->saddr);
414	return NULL;
415}
416
417int __init wg_allowedips_slab_init(void)
418{
419	node_cache = KMEM_CACHE(allowedips_node, 0);
420	return node_cache ? 0 : -ENOMEM;
421}
422
423void wg_allowedips_slab_uninit(void)
424{
425	rcu_barrier();
426	kmem_cache_destroy(node_cache);
427}
428
429#include "selftest/allowedips.c"