Linux Audio

Check our new training course

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