patricia.c

   1 /*
   2  * netsniff-ng - the packet sniffing beast
   3  * Copyright 2011 Daniel Borkmann, rewritten
   4  * Copyright 1991-2007 Kawahara Lab., Kyoto University
   5  * Copyright 2000-2005 Shikano Lab., Nara Institute of Science and Technology
   6  * Copyright 2005-2007 Julius project team, Nagoya Institute of Technology
   7  * All rights reserved
   8  * Subject to the GPL, version 2.
   9  */
  10
  11 #include <stdio.h>
  12 #include <string.h>
  13 #include <errno.h>
  14
  15 #include "patricia.h"
  16 #include "built_in.h"
  17 #include "xmalloc.h"
  18
  19 static const unsigned char mbit[] = { 0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01 };
  20
  21 static inline int testbit(char *str, size_t slen, int bitplace)
  22 {
  23         int maskptr;
  24
  25         if ((maskptr = bitplace >> 3) > slen)
  26                 return 0;
  27
  28         return (str[maskptr] & mbit[bitplace & 7]);
  29 }
  30
  31 static inline int testbit_max(char *str, int bitplace, int maxbitplace)
  32 {
  33         if (bitplace >= maxbitplace)
  34                 return 0;
  35
  36         return (str[bitplace >> 3] & mbit[bitplace & 7]);
  37 }
  38
  39 static int where_the_bit_differ(char *str1, size_t l1, char *str2, size_t l2)
  40 {
  41         int p = 0, bitloc;
  42
  43         while (str1[p] == str2[p])
  44                 p++;
  45
  46         bitloc = p * 8;
  47
  48         while (testbit(str1, l1, bitloc) == testbit(str2, l2, bitloc))
  49                 bitloc++;
  50
  51         return bitloc;
  52 }
  53
  54 static struct patricia_node *new_node(void)
  55 {
  56         struct patricia_node *n = xzmalloc(sizeof(*n));
  57
  58         n->l = n->r = NULL;
  59
  60         return n;
  61 }
  62
  63 static void free_node(struct patricia_node *n)
  64 {
  65         free(n->key);
  66         free(n->addr);
  67         xfree(n);
  68 }
  69
  70 void ptree_display(struct patricia_node *node, int level)
  71 {
  72         int i;
  73
  74         for (i = 0; i < level; ++i)
  75                 printf("-");
  76
  77         if (node->l == NULL && node->r == NULL)
  78                 printf("leaf: (%s -> %d)\n", (char *) node->key, node->value.data);
  79         else {
  80                 printf("thres: %d\n", node->value.thres_bit);
  81
  82                 if (node->l != NULL)
  83                         ptree_display(node->l, level + 1);
  84                 if (node->r != NULL)
  85                         ptree_display(node->r, level + 1);
  86         }
  87 }
  88
  89 void ptree_get_key(int data, struct patricia_node *node,
  90                    struct patricia_node **wanted)
  91 {
  92         if (!node)
  93                 return;
  94
  95         if (node->l == NULL && node->r == NULL) {
  96                 if (node->value.data == data)
  97                         (*wanted) = node;
  98         } else {
  99                 if (node->l != NULL)
 100                         ptree_get_key(data, node->l, wanted);
 101                 if (node->r != NULL)
 102                         ptree_get_key(data, node->r, wanted);
 103         }
 104 }
 105
 106 void ptree_get_key_addr(struct sockaddr_storage *addr, size_t alen,
 107                         struct patricia_node *node, struct patricia_node **wanted)
 108 {
 109         if (!node)
 110                 return;
 111
 112         if (node->l == NULL && node->r == NULL) {
 113                 if (!memcmp(node->addr, addr, node->alen))
 114                         (*wanted) = node;
 115         } else {
 116                 if (node->l != NULL)
 117                         ptree_get_key_addr(addr, alen, node->l, wanted);
 118                 if (node->r != NULL)
 119                         ptree_get_key_addr(addr, alen, node->r, wanted);
 120         }
 121 }
 122
 123 static int ptree_search_data_r(struct patricia_node *node, char *str, size_t slen,
 124                                struct sockaddr_storage *addr, size_t *alen, int maxbitplace)
 125 {
 126         if (node->l == NULL && node->r == NULL) {
 127                 if (node->addr && addr)
 128                         memcpy(addr, node->addr, node->alen);
 129
 130                 (*alen) = node->alen;
 131
 132                 return node->value.data;
 133         }
 134
 135         if (testbit_max(str, node->value.thres_bit, maxbitplace) != 0)
 136                 return ptree_search_data_r(node->r, str, slen, addr, alen, maxbitplace);
 137         else
 138                 return ptree_search_data_r(node->l, str, slen, addr, alen, maxbitplace);
 139 }
 140
 141 static int *ptree_search_data_r_p(struct patricia_node *node, char *str,
 142                                   size_t slen, int maxbitplace)
 143 {
 144         if (node->l == NULL && node->r == NULL)
 145                 return &(node->value.data);
 146
 147         if (testbit_max(str, node->value.thres_bit, maxbitplace) != 0)
 148                 return ptree_search_data_r_p(node->r, str, slen, maxbitplace);
 149         else
 150                 return ptree_search_data_r_p(node->l, str, slen, maxbitplace);
 151 }
 152
 153 static int ptree_search_data_r_x(struct patricia_node *node, char *str,
 154                                  size_t slen, struct sockaddr_storage *addr,
 155                                  size_t *alen, int maxbitplace)
 156 {
 157         if (node->l == NULL && node->r == NULL) {
 158                 if (node->klen == slen && !memcmp(str, node->key, node->klen)) {
 159                         if (node->addr && addr)
 160                                 memcpy(addr, node->addr, node->alen);
 161
 162                         (*alen) = node->alen;
 163
 164                         return node->value.data;
 165                 } else
 166                         return -ENOENT;
 167         }
 168
 169         if (testbit_max(str, node->value.thres_bit, maxbitplace) != 0)
 170                 return ptree_search_data_r_x(node->r, str, slen, addr, alen, maxbitplace);
 171         else
 172                 return ptree_search_data_r_x(node->l, str, slen, addr, alen, maxbitplace);
 173 }
 174
 175 int ptree_search_data_nearest(void *str, size_t sstr, struct sockaddr_storage *addr,
 176                               size_t *alen, struct patricia_node *root)
 177 {
 178         if (!root)
 179                 return -ENOENT;
 180
 181         return ptree_search_data_r(root, str, sstr, addr, alen, sstr * 8);
 182 }
 183
 184 static int *ptree_search_data_nearest_ptr(char *str, size_t sstr,
 185                                           struct patricia_node *root)
 186 {
 187         return ptree_search_data_r_p(root, str, sstr, sstr * 8);
 188 }
 189
 190 int ptree_search_data_exact(void *str, size_t sstr, struct sockaddr_storage *addr,
 191                             size_t *alen, struct patricia_node *root)
 192 {
 193         if (!root)
 194                 return -ENOENT;
 195
 196         return ptree_search_data_r_x(root, str, sstr, addr, alen, sstr * 8);
 197 }
 198
 199 static struct patricia_node *ptree_make_root_node(char *str, size_t sstr,
 200                                                   int data, struct sockaddr_storage *addr,
 201                                                   size_t alen)
 202 {
 203         struct patricia_node *n = new_node();
 204
 205         n->value.data = data;
 206         n->key = xmemdupz(str, sstr);
 207         n->klen = sstr;
 208         if (addr)
 209                 n->addr = xmemdupz(addr, alen);
 210         else
 211                 n->addr = NULL;
 212         n->alen = alen;
 213
 214         return n;
 215 }
 216
 217 static void ptree_add_entry_at(char *str, size_t slen, int bitloc, int data,
 218                                struct sockaddr_storage *addr, size_t alen,
 219                                struct patricia_node **parentlink)
 220 {
 221         struct patricia_node *node = (*parentlink);
 222
 223         if (node->value.thres_bit > bitloc || (node->l == NULL && node->r == NULL)) {
 224                 struct patricia_node *newleaf, *newbranch;
 225
 226                 newleaf = new_node();
 227                 newleaf->value.data = data;
 228                 newleaf->key = xmemdupz(str, slen);
 229                 newleaf->klen = slen;
 230
 231                 if (addr)
 232                         newleaf->addr = xmemdupz(addr, alen);
 233                 else
 234                         newleaf->addr = NULL;
 235                 newleaf->alen = alen;
 236
 237                 newbranch = new_node();
 238                 newbranch->value.thres_bit = bitloc;
 239                 (*parentlink) = newbranch;
 240
 241                 if (testbit(str, slen, bitloc) ==0) {
 242                         newbranch->l = newleaf;
 243                         newbranch->r = node;
 244                 } else {
 245                         newbranch->l = node;
 246                         newbranch->r = newleaf;
 247                 }
 248         } else {
 249                 if (testbit(str, slen, node->value.thres_bit) != 0)
 250                         ptree_add_entry_at(str, slen, bitloc, data, addr, alen, &(node->r));
 251                 else
 252                         ptree_add_entry_at(str, slen, bitloc, data, addr, alen, &(node->l));
 253         }
 254 }
 255
 256 int ptree_add_entry(void *str, size_t sstr, int data, struct sockaddr_storage *addr,
 257                     size_t alen, struct patricia_node **root)
 258 {
 259         int *ptr, bitloc, malicious = 0;
 260         struct patricia_node *n;
 261
 262         if (!(*root))
 263                 (*root) = ptree_make_root_node(str, sstr, data, addr, alen);
 264         else {
 265                 ptr = ptree_search_data_nearest_ptr(str, sstr, (*root));
 266                 n = container_of(ptr, struct patricia_node, value.data);
 267
 268                 if (n->klen == sstr && !memcmp(str, n->key, n->klen)) {
 269                         /* Make sure if entry exists, that we also have the
 270                          * same data, otherwise, we drop the packet */
 271                         if (n->value.data != data)
 272                                 malicious = 1;
 273                         else if (n->alen != alen)
 274                                 malicious = 1;
 275                         else if ((n->addr && !addr) || (!n->addr && addr))
 276                                 malicious = 1;
 277                         else if (n->alen == alen && n->addr && addr) {
 278                                 if (memcmp(n->addr, addr, alen))
 279                                         malicious = 1;
 280                         }
 281
 282                         return malicious;
 283                 }
 284
 285                 bitloc = where_the_bit_differ(str, sstr, n->key, n->klen);
 286                 ptree_add_entry_at(str, sstr, bitloc, data, addr, alen, root);
 287         }
 288
 289         return malicious;
 290 }
 291
 292 static void ptree_remove_entry_r(struct patricia_node *now,
 293                                  struct patricia_node *up,
 294                                  struct patricia_node *up2,
 295                                  char *str, size_t slen, int maxbitplace,
 296                                  struct patricia_node **root)
 297 {
 298         struct patricia_node *b;
 299
 300         if (now->l == NULL && now->r == NULL) {
 301                 if (now->klen != slen)
 302                         return;
 303                 if (memcmp(now->key, str, slen))
 304                         return;
 305                 if (up == NULL) {
 306                         *root = NULL;
 307                         free_node(now);
 308                         return;
 309                 }
 310
 311                 b = (up->r == now) ? up->l : up->r;
 312
 313                 if (up2 == NULL) {
 314                         *root = b;
 315                         free_node(now);
 316                         free_node(up);
 317                         return;
 318                 }
 319                 if (up2->l == up)
 320                         up2->l = b;
 321                 else
 322                         up2->r = b;
 323
 324                 free_node(now);
 325                 free_node(up);
 326         } else {
 327                 if (testbit_max(str, now->value.thres_bit, maxbitplace) != 0)
 328                         ptree_remove_entry_r(now->r, now, up, str, slen, maxbitplace, root);
 329                 else
 330                         ptree_remove_entry_r(now->l, now, up, str, slen, maxbitplace, root);
 331         }
 332 }
 333
 334 void ptree_del_entry(void *str, size_t sstr, struct patricia_node **root)
 335 {
 336         if (!(*root))
 337                 return;
 338
 339         ptree_remove_entry_r(*root, NULL, NULL, str, sstr, sstr * 8, root);
 340 }
 341
 342 void ptree_free(struct patricia_node *node)
 343 {
 344         if (!node)
 345                 return;
 346
 347         if (node->l)
 348                 ptree_free(node->l);
 349         if (node->r)
 350                 ptree_free(node->r);
 351
 352         free_node(node);
 353 }