src/patricia.c

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