drivers/net/wireguard/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
   9 static 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
  19 static 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
  34 static 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
  43 static 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
  56 static 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
  69 static unsigned int fls128(u64 a, u64 b)
  70 {
  71         return a ? fls64(a) + 64U : fls64(b);
  72 }
  73
  74 static u8 common_bits(const struct allowedips_node *node, const u8 *key,
  75                       u8 bits)
  76 {
  77         if (bits == 32)
  78                 return 32U - fls(*(const u32 *)node->bits ^ *(const u32 *)key);
  79         else if (bits == 128)
  80                 return 128U - fls128(
  81                         *(const u64 *)&node->bits[0] ^ *(const u64 *)&key[0],
  82                         *(const u64 *)&node->bits[8] ^ *(const u64 *)&key[8]);
  83         return 0;
  84 }
  85
  86 static bool prefix_matches(const struct allowedips_node *node, const u8 *key,
  87                            u8 bits)
  88 {
  89         /* This could be much faster if it actually just compared the common
  90          * bits properly, by precomputing a mask bswap(~0 << (32 - cidr)), and
  91          * the rest, but it turns out that common_bits is already super fast on
  92          * modern processors, even taking into account the unfortunate bswap.
  93          * So, we just inline it like this instead.
  94          */
  95         return common_bits(node, key, bits) >= node->cidr;
  96 }
  97
  98 static struct allowedips_node *find_node(struct allowedips_node *trie, u8 bits,
  99                                          const u8 *key)
 100 {
 101         struct allowedips_node *node = trie, *found = NULL;
 102
 103         while (node && prefix_matches(node, key, bits)) {
 104                 if (rcu_access_pointer(node->peer))
 105                         found = node;
 106                 if (node->cidr == bits)
 107                         break;
 108                 node = rcu_dereference_bh(CHOOSE_NODE(node, key));
 109         }
 110         return found;
 111 }
 112
 113 /* Returns a strong reference to a peer */
 114 static struct wg_peer *lookup(struct allowedips_node __rcu *root, u8 bits,
 115                               const void *be_ip)
 116 {
 117         /* Aligned so it can be passed to fls/fls64 */
 118         u8 ip[16] __aligned(__alignof(u64));
 119         struct allowedips_node *node;
 120         struct wg_peer *peer = NULL;
 121
 122         swap_endian(ip, be_ip, bits);
 123
 124         rcu_read_lock_bh();
 125 retry:
 126         node = find_node(rcu_dereference_bh(root), bits, ip);
 127         if (node) {
 128                 peer = wg_peer_get_maybe_zero(rcu_dereference_bh(node->peer));
 129                 if (!peer)
 130                         goto retry;
 131         }
 132         rcu_read_unlock_bh();
 133         return peer;
 134 }
 135
 136 static bool node_placement(struct allowedips_node __rcu *trie, const u8 *key,
 137                            u8 cidr, u8 bits, struct allowedips_node **rnode,
 138                            struct mutex *lock)
 139 {
 140         struct allowedips_node *node = rcu_dereference_protected(trie,
 141                                                 lockdep_is_held(lock));
 142         struct allowedips_node *parent = NULL;
 143         bool exact = false;
 144
 145         while (node && node->cidr <= cidr && prefix_matches(node, key, bits)) {
 146                 parent = node;
 147                 if (parent->cidr == cidr) {
 148                         exact = true;
 149                         break;
 150                 }
 151                 node = rcu_dereference_protected(CHOOSE_NODE(parent, key),
 152                                                  lockdep_is_held(lock));
 153         }
 154         *rnode = parent;
 155         return exact;
 156 }
 157
 158 static int add(struct allowedips_node __rcu **trie, u8 bits, const u8 *key,
 159                u8 cidr, struct wg_peer *peer, struct mutex *lock)
 160 {
 161         struct allowedips_node *node, *parent, *down, *newnode;
 162
 163         if (unlikely(cidr > bits || !peer))
 164                 return -EINVAL;
 165
 166         if (!rcu_access_pointer(*trie)) {
 167                 node = kzalloc(sizeof(*node), GFP_KERNEL);
 168                 if (unlikely(!node))
 169                         return -ENOMEM;
 170                 RCU_INIT_POINTER(node->peer, peer);
 171                 list_add_tail(&node->peer_list, &peer->allowedips_list);
 172                 copy_and_assign_cidr(node, key, cidr, bits);
 173                 rcu_assign_pointer(node->parent_bit, trie);
 174                 rcu_assign_pointer(*trie, node);
 175                 return 0;
 176         }
 177         if (node_placement(*trie, key, cidr, bits, &node, lock)) {
 178                 rcu_assign_pointer(node->peer, peer);
 179                 list_move_tail(&node->peer_list, &peer->allowedips_list);
 180                 return 0;
 181         }
 182
 183         newnode = kzalloc(sizeof(*newnode), GFP_KERNEL);
 184         if (unlikely(!newnode))
 185                 return -ENOMEM;
 186         RCU_INIT_POINTER(newnode->peer, peer);
 187         list_add_tail(&newnode->peer_list, &peer->allowedips_list);
 188         copy_and_assign_cidr(newnode, key, cidr, bits);
 189
 190         if (!node) {
 191                 down = rcu_dereference_protected(*trie, lockdep_is_held(lock));
 192         } else {
 193                 down = rcu_dereference_protected(CHOOSE_NODE(node, key), lockdep_is_held(lock));
 194                 if (!down) {
 195                         rcu_assign_pointer(newnode->parent_bit, &CHOOSE_NODE(node, key));
 196                         rcu_assign_pointer(CHOOSE_NODE(node, key), newnode);
 197                         return 0;
 198                 }
 199         }
 200         cidr = min(cidr, common_bits(down, key, bits));
 201         parent = node;
 202
 203         if (newnode->cidr == cidr) {
 204                 rcu_assign_pointer(down->parent_bit, &CHOOSE_NODE(newnode, down->bits));
 205                 rcu_assign_pointer(CHOOSE_NODE(newnode, down->bits), down);
 206                 if (!parent) {
 207                         rcu_assign_pointer(newnode->parent_bit, trie);
 208                         rcu_assign_pointer(*trie, newnode);
 209                 } else {
 210                         rcu_assign_pointer(newnode->parent_bit, &CHOOSE_NODE(parent, newnode->bits));
 211                         rcu_assign_pointer(CHOOSE_NODE(parent, newnode->bits), newnode);
 212                 }
 213                 return 0;
 214         }
 215
 216         node = kzalloc(sizeof(*node), GFP_KERNEL);
 217         if (unlikely(!node)) {
 218                 list_del(&newnode->peer_list);
 219                 kfree(newnode);
 220                 return -ENOMEM;
 221         }
 222         INIT_LIST_HEAD(&node->peer_list);
 223         copy_and_assign_cidr(node, newnode->bits, cidr, bits);
 224
 225         rcu_assign_pointer(down->parent_bit, &CHOOSE_NODE(node, down->bits));
 226         rcu_assign_pointer(CHOOSE_NODE(node, down->bits), down);
 227         rcu_assign_pointer(newnode->parent_bit, &CHOOSE_NODE(node, newnode->bits));
 228         rcu_assign_pointer(CHOOSE_NODE(node, newnode->bits), newnode);
 229         if (!parent) {
 230                 rcu_assign_pointer(node->parent_bit, trie);
 231                 rcu_assign_pointer(*trie, node);
 232         } else {
 233                 rcu_assign_pointer(node->parent_bit, &CHOOSE_NODE(parent, node->bits));
 234                 rcu_assign_pointer(CHOOSE_NODE(parent, node->bits), node);
 235         }
 236         return 0;
 237 }
 238
 239 void wg_allowedips_init(struct allowedips *table)
 240 {
 241         table->root4 = table->root6 = NULL;
 242         table->seq = 1;
 243 }
 244
 245 void wg_allowedips_free(struct allowedips *table, struct mutex *lock)
 246 {
 247         struct allowedips_node __rcu *old4 = table->root4, *old6 = table->root6;
 248
 249         ++table->seq;
 250         RCU_INIT_POINTER(table->root4, NULL);
 251         RCU_INIT_POINTER(table->root6, NULL);
 252         if (rcu_access_pointer(old4)) {
 253                 struct allowedips_node *node = rcu_dereference_protected(old4,
 254                                                         lockdep_is_held(lock));
 255
 256                 root_remove_peer_lists(node);
 257                 call_rcu(&node->rcu, root_free_rcu);
 258         }
 259         if (rcu_access_pointer(old6)) {
 260                 struct allowedips_node *node = rcu_dereference_protected(old6,
 261                                                         lockdep_is_held(lock));
 262
 263                 root_remove_peer_lists(node);
 264                 call_rcu(&node->rcu, root_free_rcu);
 265         }
 266 }
 267
 268 int wg_allowedips_insert_v4(struct allowedips *table, const struct in_addr *ip,
 269                             u8 cidr, struct wg_peer *peer, struct mutex *lock)
 270 {
 271         /* Aligned so it can be passed to fls */
 272         u8 key[4] __aligned(__alignof(u32));
 273
 274         ++table->seq;
 275         swap_endian(key, (const u8 *)ip, 32);
 276         return add(&table->root4, 32, key, cidr, peer, lock);
 277 }
 278
 279 int wg_allowedips_insert_v6(struct allowedips *table, const struct in6_addr *ip,
 280                             u8 cidr, struct wg_peer *peer, struct mutex *lock)
 281 {
 282         /* Aligned so it can be passed to fls64 */
 283         u8 key[16] __aligned(__alignof(u64));
 284
 285         ++table->seq;
 286         swap_endian(key, (const u8 *)ip, 128);
 287         return add(&table->root6, 128, key, cidr, peer, lock);
 288 }
 289
 290 void wg_allowedips_remove_by_peer(struct allowedips *table,
 291                                   struct wg_peer *peer, struct mutex *lock)
 292 {
 293         struct allowedips_node *node, *child, *tmp;
 294
 295         if (list_empty(&peer->allowedips_list))
 296                 return;
 297         ++table->seq;
 298         list_for_each_entry_safe(node, tmp, &peer->allowedips_list, peer_list) {
 299                 list_del_init(&node->peer_list);
 300                 RCU_INIT_POINTER(node->peer, NULL);
 301                 if (node->bit[0] && node->bit[1])
 302                         continue;
 303                 child = rcu_dereference_protected(
 304                                 node->bit[!rcu_access_pointer(node->bit[0])],
 305                                 lockdep_is_held(lock));
 306                 if (child)
 307                         child->parent_bit = node->parent_bit;
 308                 *rcu_dereference_protected(node->parent_bit, lockdep_is_held(lock)) = child;
 309                 kfree_rcu(node, rcu);
 310
 311                 /* TODO: Note that we currently don't walk up and down in order to
 312                  * free any potential filler nodes. This means that this function
 313                  * doesn't free up as much as it could, which could be revisited
 314                  * at some point.
 315                  */
 316         }
 317 }
 318
 319 int wg_allowedips_read_node(struct allowedips_node *node, u8 ip[16], u8 *cidr)
 320 {
 321         const unsigned int cidr_bytes = DIV_ROUND_UP(node->cidr, 8U);
 322         swap_endian(ip, node->bits, node->bitlen);
 323         memset(ip + cidr_bytes, 0, node->bitlen / 8U - cidr_bytes);
 324         if (node->cidr)
 325                 ip[cidr_bytes - 1U] &= ~0U << (-node->cidr % 8U);
 326
 327         *cidr = node->cidr;
 328         return node->bitlen == 32 ? AF_INET : AF_INET6;
 329 }
 330
 331 /* Returns a strong reference to a peer */
 332 struct wg_peer *wg_allowedips_lookup_dst(struct allowedips *table,
 333                                          struct sk_buff *skb)
 334 {
 335         if (skb->protocol == htons(ETH_P_IP))
 336                 return lookup(table->root4, 32, &ip_hdr(skb)->daddr);
 337         else if (skb->protocol == htons(ETH_P_IPV6))
 338                 return lookup(table->root6, 128, &ipv6_hdr(skb)->daddr);
 339         return NULL;
 340 }
 341
 342 /* Returns a strong reference to a peer */
 343 struct wg_peer *wg_allowedips_lookup_src(struct allowedips *table,
 344                                          struct sk_buff *skb)
 345 {
 346         if (skb->protocol == htons(ETH_P_IP))
 347                 return lookup(table->root4, 32, &ip_hdr(skb)->saddr);
 348         else if (skb->protocol == htons(ETH_P_IPV6))
 349                 return lookup(table->root6, 128, &ipv6_hdr(skb)->saddr);
 350         return NULL;
 351 }
 352
 353 #include "selftest/allowedips.c"