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 9 enum { MAX_ALLOWEDIPS_DEPTH = 129 }; 10 11 static struct kmem_cache *node_cache; 12 13 static 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 23 static 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 36 static 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 41 static 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 51 static void node_free_rcu(struct rcu_head *rcu) 52 { 53 kmem_cache_free(node_cache, container_of(rcu, struct allowedips_node, rcu)); 54 } 55 56 static 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 69 static 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 82 static unsigned int fls128(u64 a, u64 b) 83 { 84 return a ? fls64(a) + 64U : fls64(b); 85 } 86 87 static 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 99 static 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 111 static 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 */ 127 static 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(); 138 retry: 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 149 static 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 169 static 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 175 static 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 181 static 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 252 static 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 283 static 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 298 void wg_allowedips_init(struct allowedips *table) 299 { 300 table->root4 = table->root6 = NULL; 301 table->seq = 1; 302 } 303 304 void 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 327 int 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 338 int 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 349 int 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 360 int 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 371 void 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 383 int 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 */ 396 struct 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 */ 407 struct 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 417 int __init wg_allowedips_slab_init(void) 418 { 419 node_cache = KMEM_CACHE(allowedips_node, 0); 420 return node_cache ? 0 : -ENOMEM; 421 } 422 423 void wg_allowedips_slab_uninit(void) 424 { 425 rcu_barrier(); 426 kmem_cache_destroy(node_cache); 427 } 428 429 #include "selftest/allowedips.c" 430