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 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
 123 static unsigned int fls128(u64 a, u64 b)
 124 {
 125         return a ? fls64(a) + 64U : fls64(b);
 126 }
 127
 128 static 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
 140 static 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
 152 static 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 */
 168 static 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();
 179 retry:
 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
 190 static 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
 212 static 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
 284 void wg_allowedips_init(struct allowedips *table)
 285 {
 286         table->root4 = table->root6 = NULL;
 287         table->seq = 1;
 288 }
 289
 290 void 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
 313 int 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
 324 int 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
 335 void 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
 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 #include "selftest/allowedips.c"