drivers/net/wireguard/allowedips.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (C) 2015-2019 Jason A. Donenfeld <[email protected]>. All Rights Reserved.
   4  */
   5
   6 #include "allowedips.h"
   7 #include "peer.h"
   8
   9 enum { MAX_ALLOWEDIPS_BITS = 128 };
  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] = be64_to_cpu(((const __be64 *)src)[0]);
  19                 ((u64 *)dst)[1] = be64_to_cpu(((const __be64 *)src)[1]);
  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_BITS))
  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_BITS] = {
  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_BITS] = { 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 void wg_allowedips_init(struct allowedips *table)
 253 {
 254         table->root4 = table->root6 = NULL;
 255         table->seq = 1;
 256 }
 257
 258 void wg_allowedips_free(struct allowedips *table, struct mutex *lock)
 259 {
 260         struct allowedips_node __rcu *old4 = table->root4, *old6 = table->root6;
 261
 262         ++table->seq;
 263         RCU_INIT_POINTER(table->root4, NULL);
 264         RCU_INIT_POINTER(table->root6, NULL);
 265         if (rcu_access_pointer(old4)) {
 266                 struct allowedips_node *node = rcu_dereference_protected(old4,
 267                                                         lockdep_is_held(lock));
 268
 269                 root_remove_peer_lists(node);
 270                 call_rcu(&node->rcu, root_free_rcu);
 271         }
 272         if (rcu_access_pointer(old6)) {
 273                 struct allowedips_node *node = rcu_dereference_protected(old6,
 274                                                         lockdep_is_held(lock));
 275
 276                 root_remove_peer_lists(node);
 277                 call_rcu(&node->rcu, root_free_rcu);
 278         }
 279 }
 280
 281 int wg_allowedips_insert_v4(struct allowedips *table, const struct in_addr *ip,
 282                             u8 cidr, struct wg_peer *peer, struct mutex *lock)
 283 {
 284         /* Aligned so it can be passed to fls */
 285         u8 key[4] __aligned(__alignof(u32));
 286
 287         ++table->seq;
 288         swap_endian(key, (const u8 *)ip, 32);
 289         return add(&table->root4, 32, key, cidr, peer, lock);
 290 }
 291
 292 int wg_allowedips_insert_v6(struct allowedips *table, const struct in6_addr *ip,
 293                             u8 cidr, struct wg_peer *peer, struct mutex *lock)
 294 {
 295         /* Aligned so it can be passed to fls64 */
 296         u8 key[16] __aligned(__alignof(u64));
 297
 298         ++table->seq;
 299         swap_endian(key, (const u8 *)ip, 128);
 300         return add(&table->root6, 128, key, cidr, peer, lock);
 301 }
 302
 303 void wg_allowedips_remove_by_peer(struct allowedips *table,
 304                                   struct wg_peer *peer, struct mutex *lock)
 305 {
 306         struct allowedips_node *node, *child, **parent_bit, *parent, *tmp;
 307         bool free_parent;
 308
 309         if (list_empty(&peer->allowedips_list))
 310                 return;
 311         ++table->seq;
 312         list_for_each_entry_safe(node, tmp, &peer->allowedips_list, peer_list) {
 313                 list_del_init(&node->peer_list);
 314                 RCU_INIT_POINTER(node->peer, NULL);
 315                 if (node->bit[0] && node->bit[1])
 316                         continue;
 317                 child = rcu_dereference_protected(node->bit[!rcu_access_pointer(node->bit[0])],
 318                                                   lockdep_is_held(lock));
 319                 if (child)
 320                         child->parent_bit_packed = node->parent_bit_packed;
 321                 parent_bit = (struct allowedips_node **)(node->parent_bit_packed & ~3UL);
 322                 *parent_bit = child;
 323                 parent = (void *)parent_bit -
 324                          offsetof(struct allowedips_node, bit[node->parent_bit_packed & 1]);
 325                 free_parent = !rcu_access_pointer(node->bit[0]) &&
 326                               !rcu_access_pointer(node->bit[1]) &&
 327                               (node->parent_bit_packed & 3) <= 1 &&
 328                               !rcu_access_pointer(parent->peer);
 329                 if (free_parent)
 330                         child = rcu_dereference_protected(
 331                                         parent->bit[!(node->parent_bit_packed & 1)],
 332                                         lockdep_is_held(lock));
 333                 call_rcu(&node->rcu, node_free_rcu);
 334                 if (!free_parent)
 335                         continue;
 336                 if (child)
 337                         child->parent_bit_packed = parent->parent_bit_packed;
 338                 *(struct allowedips_node **)(parent->parent_bit_packed & ~3UL) = child;
 339                 call_rcu(&parent->rcu, node_free_rcu);
 340         }
 341 }
 342
 343 int 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 */
 356 struct 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 */
 367 struct 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 int __init wg_allowedips_slab_init(void)
 378 {
 379         node_cache = KMEM_CACHE(allowedips_node, 0);
 380         return node_cache ? 0 : -ENOMEM;
 381 }
 382
 383 void wg_allowedips_slab_uninit(void)
 384 {
 385         rcu_barrier();
 386         kmem_cache_destroy(node_cache);
 387 }
 388
 389 #include "selftest/allowedips.c"