forked from Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
377 lines
10 KiB
377 lines
10 KiB
// SPDX-License-Identifier: GPL-2.0 |
|
/* |
|
* Copyright (C) 2015-2019 Jason A. Donenfeld <[email protected]>. All Rights Reserved. |
|
*/ |
|
|
|
#include "allowedips.h" |
|
#include "peer.h" |
|
|
|
static void swap_endian(u8 *dst, const u8 *src, u8 bits) |
|
{ |
|
if (bits == 32) { |
|
*(u32 *)dst = be32_to_cpu(*(const __be32 *)src); |
|
} else if (bits == 128) { |
|
((u64 *)dst)[0] = be64_to_cpu(((const __be64 *)src)[0]); |
|
((u64 *)dst)[1] = be64_to_cpu(((const __be64 *)src)[1]); |
|
} |
|
} |
|
|
|
static void copy_and_assign_cidr(struct allowedips_node *node, const u8 *src, |
|
u8 cidr, u8 bits) |
|
{ |
|
node->cidr = cidr; |
|
node->bit_at_a = cidr / 8U; |
|
#ifdef __LITTLE_ENDIAN |
|
node->bit_at_a ^= (bits / 8U - 1U) % 8U; |
|
#endif |
|
node->bit_at_b = 7U - (cidr % 8U); |
|
node->bitlen = bits; |
|
memcpy(node->bits, src, bits / 8U); |
|
} |
|
#define CHOOSE_NODE(parent, key) \ |
|
parent->bit[(key[parent->bit_at_a] >> parent->bit_at_b) & 1] |
|
|
|
static void push_rcu(struct allowedips_node **stack, |
|
struct allowedips_node __rcu *p, unsigned int *len) |
|
{ |
|
if (rcu_access_pointer(p)) { |
|
WARN_ON(IS_ENABLED(DEBUG) && *len >= 128); |
|
stack[(*len)++] = rcu_dereference_raw(p); |
|
} |
|
} |
|
|
|
static void root_free_rcu(struct rcu_head *rcu) |
|
{ |
|
struct allowedips_node *node, *stack[128] = { |
|
container_of(rcu, struct allowedips_node, rcu) }; |
|
unsigned int len = 1; |
|
|
|
while (len > 0 && (node = stack[--len])) { |
|
push_rcu(stack, node->bit[0], &len); |
|
push_rcu(stack, node->bit[1], &len); |
|
kfree(node); |
|
} |
|
} |
|
|
|
static void root_remove_peer_lists(struct allowedips_node *root) |
|
{ |
|
struct allowedips_node *node, *stack[128] = { root }; |
|
unsigned int len = 1; |
|
|
|
while (len > 0 && (node = stack[--len])) { |
|
push_rcu(stack, node->bit[0], &len); |
|
push_rcu(stack, node->bit[1], &len); |
|
if (rcu_access_pointer(node->peer)) |
|
list_del(&node->peer_list); |
|
} |
|
} |
|
|
|
static void walk_remove_by_peer(struct allowedips_node __rcu **top, |
|
struct wg_peer *peer, struct mutex *lock) |
|
{ |
|
#define REF(p) rcu_access_pointer(p) |
|
#define DEREF(p) rcu_dereference_protected(*(p), lockdep_is_held(lock)) |
|
#define PUSH(p) ({ \ |
|
WARN_ON(IS_ENABLED(DEBUG) && len >= 128); \ |
|
stack[len++] = p; \ |
|
}) |
|
|
|
struct allowedips_node __rcu **stack[128], **nptr; |
|
struct allowedips_node *node, *prev; |
|
unsigned int len; |
|
|
|
if (unlikely(!peer || !REF(*top))) |
|
return; |
|
|
|
for (prev = NULL, len = 0, PUSH(top); len > 0; prev = node) { |
|
nptr = stack[len - 1]; |
|
node = DEREF(nptr); |
|
if (!node) { |
|
--len; |
|
continue; |
|
} |
|
if (!prev || REF(prev->bit[0]) == node || |
|
REF(prev->bit[1]) == node) { |
|
if (REF(node->bit[0])) |
|
PUSH(&node->bit[0]); |
|
else if (REF(node->bit[1])) |
|
PUSH(&node->bit[1]); |
|
} else if (REF(node->bit[0]) == prev) { |
|
if (REF(node->bit[1])) |
|
PUSH(&node->bit[1]); |
|
} else { |
|
if (rcu_dereference_protected(node->peer, |
|
lockdep_is_held(lock)) == peer) { |
|
RCU_INIT_POINTER(node->peer, NULL); |
|
list_del_init(&node->peer_list); |
|
if (!node->bit[0] || !node->bit[1]) { |
|
rcu_assign_pointer(*nptr, DEREF( |
|
&node->bit[!REF(node->bit[0])])); |
|
kfree_rcu(node, rcu); |
|
node = DEREF(nptr); |
|
} |
|
} |
|
--len; |
|
} |
|
} |
|
|
|
#undef REF |
|
#undef DEREF |
|
#undef PUSH |
|
} |
|
|
|
static unsigned int fls128(u64 a, u64 b) |
|
{ |
|
return a ? fls64(a) + 64U : fls64(b); |
|
} |
|
|
|
static u8 common_bits(const struct allowedips_node *node, const u8 *key, |
|
u8 bits) |
|
{ |
|
if (bits == 32) |
|
return 32U - fls(*(const u32 *)node->bits ^ *(const u32 *)key); |
|
else if (bits == 128) |
|
return 128U - fls128( |
|
*(const u64 *)&node->bits[0] ^ *(const u64 *)&key[0], |
|
*(const u64 *)&node->bits[8] ^ *(const u64 *)&key[8]); |
|
return 0; |
|
} |
|
|
|
static bool prefix_matches(const struct allowedips_node *node, const u8 *key, |
|
u8 bits) |
|
{ |
|
/* This could be much faster if it actually just compared the common |
|
* bits properly, by precomputing a mask bswap(~0 << (32 - cidr)), and |
|
* the rest, but it turns out that common_bits is already super fast on |
|
* modern processors, even taking into account the unfortunate bswap. |
|
* So, we just inline it like this instead. |
|
*/ |
|
return common_bits(node, key, bits) >= node->cidr; |
|
} |
|
|
|
static struct allowedips_node *find_node(struct allowedips_node *trie, u8 bits, |
|
const u8 *key) |
|
{ |
|
struct allowedips_node *node = trie, *found = NULL; |
|
|
|
while (node && prefix_matches(node, key, bits)) { |
|
if (rcu_access_pointer(node->peer)) |
|
found = node; |
|
if (node->cidr == bits) |
|
break; |
|
node = rcu_dereference_bh(CHOOSE_NODE(node, key)); |
|
} |
|
return found; |
|
} |
|
|
|
/* Returns a strong reference to a peer */ |
|
static struct wg_peer *lookup(struct allowedips_node __rcu *root, u8 bits, |
|
const void *be_ip) |
|
{ |
|
/* Aligned so it can be passed to fls/fls64 */ |
|
u8 ip[16] __aligned(__alignof(u64)); |
|
struct allowedips_node *node; |
|
struct wg_peer *peer = NULL; |
|
|
|
swap_endian(ip, be_ip, bits); |
|
|
|
rcu_read_lock_bh(); |
|
retry: |
|
node = find_node(rcu_dereference_bh(root), bits, ip); |
|
if (node) { |
|
peer = wg_peer_get_maybe_zero(rcu_dereference_bh(node->peer)); |
|
if (!peer) |
|
goto retry; |
|
} |
|
rcu_read_unlock_bh(); |
|
return peer; |
|
} |
|
|
|
static bool node_placement(struct allowedips_node __rcu *trie, const u8 *key, |
|
u8 cidr, u8 bits, struct allowedips_node **rnode, |
|
struct mutex *lock) |
|
{ |
|
struct allowedips_node *node = rcu_dereference_protected(trie, |
|
lockdep_is_held(lock)); |
|
struct allowedips_node *parent = NULL; |
|
bool exact = false; |
|
|
|
while (node && node->cidr <= cidr && prefix_matches(node, key, bits)) { |
|
parent = node; |
|
if (parent->cidr == cidr) { |
|
exact = true; |
|
break; |
|
} |
|
node = rcu_dereference_protected(CHOOSE_NODE(parent, key), |
|
lockdep_is_held(lock)); |
|
} |
|
*rnode = parent; |
|
return exact; |
|
} |
|
|
|
static int add(struct allowedips_node __rcu **trie, u8 bits, const u8 *key, |
|
u8 cidr, struct wg_peer *peer, struct mutex *lock) |
|
{ |
|
struct allowedips_node *node, *parent, *down, *newnode; |
|
|
|
if (unlikely(cidr > bits || !peer)) |
|
return -EINVAL; |
|
|
|
if (!rcu_access_pointer(*trie)) { |
|
node = kzalloc(sizeof(*node), GFP_KERNEL); |
|
if (unlikely(!node)) |
|
return -ENOMEM; |
|
RCU_INIT_POINTER(node->peer, peer); |
|
list_add_tail(&node->peer_list, &peer->allowedips_list); |
|
copy_and_assign_cidr(node, key, cidr, bits); |
|
rcu_assign_pointer(*trie, node); |
|
return 0; |
|
} |
|
if (node_placement(*trie, key, cidr, bits, &node, lock)) { |
|
rcu_assign_pointer(node->peer, peer); |
|
list_move_tail(&node->peer_list, &peer->allowedips_list); |
|
return 0; |
|
} |
|
|
|
newnode = kzalloc(sizeof(*newnode), GFP_KERNEL); |
|
if (unlikely(!newnode)) |
|
return -ENOMEM; |
|
RCU_INIT_POINTER(newnode->peer, peer); |
|
list_add_tail(&newnode->peer_list, &peer->allowedips_list); |
|
copy_and_assign_cidr(newnode, key, cidr, bits); |
|
|
|
if (!node) { |
|
down = rcu_dereference_protected(*trie, lockdep_is_held(lock)); |
|
} else { |
|
down = rcu_dereference_protected(CHOOSE_NODE(node, key), |
|
lockdep_is_held(lock)); |
|
if (!down) { |
|
rcu_assign_pointer(CHOOSE_NODE(node, key), newnode); |
|
return 0; |
|
} |
|
} |
|
cidr = min(cidr, common_bits(down, key, bits)); |
|
parent = node; |
|
|
|
if (newnode->cidr == cidr) { |
|
rcu_assign_pointer(CHOOSE_NODE(newnode, down->bits), down); |
|
if (!parent) |
|
rcu_assign_pointer(*trie, newnode); |
|
else |
|
rcu_assign_pointer(CHOOSE_NODE(parent, newnode->bits), |
|
newnode); |
|
} else { |
|
node = kzalloc(sizeof(*node), GFP_KERNEL); |
|
if (unlikely(!node)) { |
|
list_del(&newnode->peer_list); |
|
kfree(newnode); |
|
return -ENOMEM; |
|
} |
|
INIT_LIST_HEAD(&node->peer_list); |
|
copy_and_assign_cidr(node, newnode->bits, cidr, bits); |
|
|
|
rcu_assign_pointer(CHOOSE_NODE(node, down->bits), down); |
|
rcu_assign_pointer(CHOOSE_NODE(node, newnode->bits), newnode); |
|
if (!parent) |
|
rcu_assign_pointer(*trie, node); |
|
else |
|
rcu_assign_pointer(CHOOSE_NODE(parent, node->bits), |
|
node); |
|
} |
|
return 0; |
|
} |
|
|
|
void wg_allowedips_init(struct allowedips *table) |
|
{ |
|
table->root4 = table->root6 = NULL; |
|
table->seq = 1; |
|
} |
|
|
|
void wg_allowedips_free(struct allowedips *table, struct mutex *lock) |
|
{ |
|
struct allowedips_node __rcu *old4 = table->root4, *old6 = table->root6; |
|
|
|
++table->seq; |
|
RCU_INIT_POINTER(table->root4, NULL); |
|
RCU_INIT_POINTER(table->root6, NULL); |
|
if (rcu_access_pointer(old4)) { |
|
struct allowedips_node *node = rcu_dereference_protected(old4, |
|
lockdep_is_held(lock)); |
|
|
|
root_remove_peer_lists(node); |
|
call_rcu(&node->rcu, root_free_rcu); |
|
} |
|
if (rcu_access_pointer(old6)) { |
|
struct allowedips_node *node = rcu_dereference_protected(old6, |
|
lockdep_is_held(lock)); |
|
|
|
root_remove_peer_lists(node); |
|
call_rcu(&node->rcu, root_free_rcu); |
|
} |
|
} |
|
|
|
int wg_allowedips_insert_v4(struct allowedips *table, const struct in_addr *ip, |
|
u8 cidr, struct wg_peer *peer, struct mutex *lock) |
|
{ |
|
/* Aligned so it can be passed to fls */ |
|
u8 key[4] __aligned(__alignof(u32)); |
|
|
|
++table->seq; |
|
swap_endian(key, (const u8 *)ip, 32); |
|
return add(&table->root4, 32, key, cidr, peer, lock); |
|
} |
|
|
|
int wg_allowedips_insert_v6(struct allowedips *table, const struct in6_addr *ip, |
|
u8 cidr, struct wg_peer *peer, struct mutex *lock) |
|
{ |
|
/* Aligned so it can be passed to fls64 */ |
|
u8 key[16] __aligned(__alignof(u64)); |
|
|
|
++table->seq; |
|
swap_endian(key, (const u8 *)ip, 128); |
|
return add(&table->root6, 128, key, cidr, peer, lock); |
|
} |
|
|
|
void wg_allowedips_remove_by_peer(struct allowedips *table, |
|
struct wg_peer *peer, struct mutex *lock) |
|
{ |
|
++table->seq; |
|
walk_remove_by_peer(&table->root4, peer, lock); |
|
walk_remove_by_peer(&table->root6, peer, lock); |
|
} |
|
|
|
int wg_allowedips_read_node(struct allowedips_node *node, u8 ip[16], u8 *cidr) |
|
{ |
|
const unsigned int cidr_bytes = DIV_ROUND_UP(node->cidr, 8U); |
|
swap_endian(ip, node->bits, node->bitlen); |
|
memset(ip + cidr_bytes, 0, node->bitlen / 8U - cidr_bytes); |
|
if (node->cidr) |
|
ip[cidr_bytes - 1U] &= ~0U << (-node->cidr % 8U); |
|
|
|
*cidr = node->cidr; |
|
return node->bitlen == 32 ? AF_INET : AF_INET6; |
|
} |
|
|
|
/* Returns a strong reference to a peer */ |
|
struct wg_peer *wg_allowedips_lookup_dst(struct allowedips *table, |
|
struct sk_buff *skb) |
|
{ |
|
if (skb->protocol == htons(ETH_P_IP)) |
|
return lookup(table->root4, 32, &ip_hdr(skb)->daddr); |
|
else if (skb->protocol == htons(ETH_P_IPV6)) |
|
return lookup(table->root6, 128, &ipv6_hdr(skb)->daddr); |
|
return NULL; |
|
} |
|
|
|
/* Returns a strong reference to a peer */ |
|
struct wg_peer *wg_allowedips_lookup_src(struct allowedips *table, |
|
struct sk_buff *skb) |
|
{ |
|
if (skb->protocol == htons(ETH_P_IP)) |
|
return lookup(table->root4, 32, &ip_hdr(skb)->saddr); |
|
else if (skb->protocol == htons(ETH_P_IPV6)) |
|
return lookup(table->root6, 128, &ipv6_hdr(skb)->saddr); |
|
return NULL; |
|
} |
|
|
|
#include "selftest/allowedips.c"
|
|
|