sys/netpfil/ipfw/dn_heap.c

   1 /*-
   2  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
   3  *
   4  * Copyright (c) 1998-2002,2010 Luigi Rizzo, Universita` di Pisa
   5  * All rights reserved
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions
   9  * are met:
  10  * 1. Redistributions of source code must retain the above copyright
  11  *    notice, this list of conditions and the following disclaimer.
  12  * 2. Redistributions in binary form must reproduce the above copyright
  13  *    notice, this list of conditions and the following disclaimer in the
  14  *    documentation and/or other materials provided with the distribution.
  15  *
  16  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  18  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  19  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  20  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  21  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  22  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  24  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  25  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  26  * SUCH DAMAGE.
  27  */
  28
  29 /*
  30  * Binary heap and hash tables, used in dummynet
  31  *
  32  * $FreeBSD$
  33  */
  34
  35 #include <sys/cdefs.h>
  36 #include <sys/param.h>
  37 #ifdef _KERNEL
  38 __FBSDID("$FreeBSD$");
  39 #include <sys/systm.h>
  40 #include <sys/malloc.h>
  41 #include <sys/kernel.h>
  42 #include <netpfil/ipfw/dn_heap.h>
  43 #ifndef log
  44 #define log(x, arg...)
  45 #endif
  46
  47 #else /* !_KERNEL */
  48
  49 #include <stdio.h>
  50 #include <dn_test.h>
  51 #include <strings.h>
  52 #include <stdlib.h>
  53
  54 #include  "dn_heap.h"
  55 #define log(x, arg...)  fprintf(stderr, ## arg)
  56 #define panic(x...)     fprintf(stderr, ## x), exit(1)
  57 #define MALLOC_DEFINE(a, b, c)  volatile int __dummy__ ## a __attribute__((__unused__))
  58 static void *my_malloc(int s) { return malloc(s); }
  59 static void my_free(void *p) {  free(p); }
  60 #define malloc(s, t, w) my_malloc(s)
  61 #define free(p, t)      my_free(p)
  62 #endif /* !_KERNEL */
  63
  64 static MALLOC_DEFINE(M_DN_HEAP, "dummynet", "dummynet heap");
  65
  66 /*
  67  * Heap management functions.
  68  *
  69  * In the heap, first node is element 0. Children of i are 2i+1 and 2i+2.
  70  * Some macros help finding parent/children so we can optimize them.
  71  *
  72  * heap_init() is called to expand the heap when needed.
  73  * Increment size in blocks of 16 entries.
  74  * Returns 1 on error, 0 on success
  75  */
  76 #define HEAP_FATHER(x) ( ( (x) - 1 ) / 2 )
  77 #define HEAP_LEFT(x) ( (x)+(x) + 1 )
  78 #define HEAP_SWAP(a, b, buffer) { buffer = a ; a = b ; b = buffer ; }
  79 #define HEAP_INCREMENT  15
  80
  81 static int
  82 heap_resize(struct dn_heap *h, unsigned int new_size)
  83 {
  84         struct dn_heap_entry *p;
  85
  86         if ((unsigned int)h->size >= new_size ) /* have enough room */
  87                 return 0;
  88 #if 1  /* round to the next power of 2 */
  89         new_size |= new_size >> 1;
  90         new_size |= new_size >> 2;
  91         new_size |= new_size >> 4;
  92         new_size |= new_size >> 8;
  93         new_size |= new_size >> 16;
  94 #else
  95         new_size = (new_size + HEAP_INCREMENT ) & ~HEAP_INCREMENT;
  96 #endif
  97         p = mallocarray(new_size, sizeof(*p), M_DN_HEAP, M_NOWAIT);
  98         if (p == NULL) {
  99                 printf("--- %s, resize %d failed\n", __func__, new_size );
 100                 return 1; /* error */
 101         }
 102         if (h->size > 0) {
 103                 bcopy(h->p, p, h->size * sizeof(*p) );
 104                 free(h->p, M_DN_HEAP);
 105         }
 106         h->p = p;
 107         h->size = new_size;
 108         return 0;
 109 }
 110
 111 int
 112 heap_init(struct dn_heap *h, int size, int ofs)
 113 {
 114         if (heap_resize(h, size))
 115                 return 1;
 116         h->elements = 0;
 117         h->ofs = ofs;
 118         return 0;
 119 }
 120
 121 /*
 122  * Insert element in heap. Normally, p != NULL, we insert p in
 123  * a new position and bubble up. If p == NULL, then the element is
 124  * already in place, and key is the position where to start the
 125  * bubble-up.
 126  * Returns 1 on failure (cannot allocate new heap entry)
 127  *
 128  * If ofs > 0 the position (index, int) of the element in the heap is
 129  * also stored in the element itself at the given offset in bytes.
 130  */
 131 #define SET_OFFSET(h, i) do {                                   \
 132         if (h->ofs > 0)                                         \
 133             *((int32_t *)((char *)(h->p[i].object) + h->ofs)) = i;      \
 134         } while (0)
 135 /*
 136  * RESET_OFFSET is used for sanity checks. It sets ofs
 137  * to an invalid value.
 138  */
 139 #define RESET_OFFSET(h, i) do {                                 \
 140         if (h->ofs > 0)                                         \
 141             *((int32_t *)((char *)(h->p[i].object) + h->ofs)) = -16;    \
 142         } while (0)
 143
 144 int
 145 heap_insert(struct dn_heap *h, uint64_t key1, void *p)
 146 {
 147         int son = h->elements;
 148
 149         //log("%s key %llu p %p\n", __FUNCTION__, key1, p);
 150         if (p == NULL) { /* data already there, set starting point */
 151                 son = key1;
 152         } else { /* insert new element at the end, possibly resize */
 153                 son = h->elements;
 154                 if (son == h->size) /* need resize... */
 155                         // XXX expand by 16 or so
 156                         if (heap_resize(h, h->elements+16) )
 157                                 return 1; /* failure... */
 158                 h->p[son].object = p;
 159                 h->p[son].key = key1;
 160                 h->elements++;
 161         }
 162         /* make sure that son >= father along the path */
 163         while (son > 0) {
 164                 int father = HEAP_FATHER(son);
 165                 struct dn_heap_entry tmp;
 166
 167                 if (DN_KEY_LT( h->p[father].key, h->p[son].key ) )
 168                         break; /* found right position */
 169                 /* son smaller than father, swap and repeat */
 170                 HEAP_SWAP(h->p[son], h->p[father], tmp);
 171                 SET_OFFSET(h, son);
 172                 son = father;
 173         }
 174         SET_OFFSET(h, son);
 175         return 0;
 176 }
 177
 178 /*
 179  * remove top element from heap, or obj if obj != NULL
 180  */
 181 void
 182 heap_extract(struct dn_heap *h, void *obj)
 183 {
 184         int child, father, max = h->elements - 1;
 185
 186         if (max < 0) {
 187                 printf("--- %s: empty heap 0x%p\n", __FUNCTION__, h);
 188                 return;
 189         }
 190         if (obj == NULL)
 191                 father = 0; /* default: move up smallest child */
 192         else { /* extract specific element, index is at offset */
 193                 if (h->ofs <= 0)
 194                         panic("%s: extract from middle not set on %p\n",
 195                                 __FUNCTION__, h);
 196                 father = *((int *)((char *)obj + h->ofs));
 197                 if (father < 0 || father >= h->elements) {
 198                         panic("%s: father %d out of bound 0..%d\n",
 199                                 __FUNCTION__, father, h->elements);
 200                 }
 201         }
 202         /*
 203          * below, father is the index of the empty element, which
 204          * we replace at each step with the smallest child until we
 205          * reach the bottom level.
 206          */
 207         // XXX why removing RESET_OFFSET increases runtime by 10% ?
 208         RESET_OFFSET(h, father);
 209         while ( (child = HEAP_LEFT(father)) <= max ) {
 210                 if (child != max &&
 211                     DN_KEY_LT(h->p[child+1].key, h->p[child].key) )
 212                         child++; /* take right child, otherwise left */
 213                 h->p[father] = h->p[child];
 214                 SET_OFFSET(h, father);
 215                 father = child;
 216         }
 217         h->elements--;
 218         if (father != max) {
 219                 /*
 220                  * Fill hole with last entry and bubble up,
 221                  * reusing the insert code
 222                  */
 223                 h->p[father] = h->p[max];
 224                 heap_insert(h, father, NULL);
 225         }
 226 }
 227
 228 #if 0
 229 /*
 230  * change object position and update references
 231  * XXX this one is never used!
 232  */
 233 static void
 234 heap_move(struct dn_heap *h, uint64_t new_key, void *object)
 235 {
 236         int temp, i, max = h->elements-1;
 237         struct dn_heap_entry *p, buf;
 238
 239         if (h->ofs <= 0)
 240                 panic("cannot move items on this heap");
 241         p = h->p;       /* shortcut */
 242
 243         i = *((int *)((char *)object + h->ofs));
 244         if (DN_KEY_LT(new_key, p[i].key) ) { /* must move up */
 245                 p[i].key = new_key;
 246                 for (; i>0 &&
 247                     DN_KEY_LT(new_key, p[(temp = HEAP_FATHER(i))].key);
 248                     i = temp ) { /* bubble up */
 249                         HEAP_SWAP(p[i], p[temp], buf);
 250                         SET_OFFSET(h, i);
 251                 }
 252         } else {                /* must move down */
 253                 p[i].key = new_key;
 254                 while ( (temp = HEAP_LEFT(i)) <= max ) {
 255                         /* found left child */
 256                         if (temp != max &&
 257                             DN_KEY_LT(p[temp+1].key, p[temp].key))
 258                                 temp++; /* select child with min key */
 259                         if (DN_KEY_LT(>p[temp].key, new_key)) {
 260                                 /* go down */
 261                                 HEAP_SWAP(p[i], p[temp], buf);
 262                                 SET_OFFSET(h, i);
 263                         } else
 264                                 break;
 265                         i = temp;
 266                 }
 267         }
 268         SET_OFFSET(h, i);
 269 }
 270 #endif /* heap_move, unused */
 271
 272 /*
 273  * heapify() will reorganize data inside an array to maintain the
 274  * heap property. It is needed when we delete a bunch of entries.
 275  */
 276 static void
 277 heapify(struct dn_heap *h)
 278 {
 279         int i;
 280
 281         for (i = 0; i < h->elements; i++ )
 282                 heap_insert(h, i , NULL);
 283 }
 284
 285 int
 286 heap_scan(struct dn_heap *h, int (*fn)(void *, uintptr_t),
 287         uintptr_t arg)
 288 {
 289         int i, ret, found;
 290
 291         for (i = found = 0 ; i < h->elements ;) {
 292                 ret = fn(h->p[i].object, arg);
 293                 if (ret & HEAP_SCAN_DEL) {
 294                         h->elements-- ;
 295                         h->p[i] = h->p[h->elements] ;
 296                         found++ ;
 297                 } else
 298                         i++ ;
 299                 if (ret & HEAP_SCAN_END)
 300                         break;
 301         }
 302         if (found)
 303                 heapify(h);
 304         return found;
 305 }
 306
 307 /*
 308  * cleanup the heap and free data structure
 309  */
 310 void
 311 heap_free(struct dn_heap *h)
 312 {
 313         if (h->size >0 )
 314                 free(h->p, M_DN_HEAP);
 315         bzero(h, sizeof(*h) );
 316 }
 317
 318 /*
 319  * hash table support.
 320  */
 321
 322 struct dn_ht {
 323         int buckets;            /* how many buckets, really buckets - 1*/
 324         int entries;            /* how many entries */
 325         int ofs;                /* offset of link field */
 326         uint32_t (*hash)(uintptr_t, int, void *arg);
 327         int (*match)(void *_el, uintptr_t key, int, void *);
 328         void *(*newh)(uintptr_t, int, void *);
 329         void **ht;              /* bucket heads */
 330 };
 331 /*
 332  * Initialize, allocating bucket pointers inline.
 333  * Recycle previous record if possible.
 334  * If the 'newh' function is not supplied, we assume that the
 335  * key passed to ht_find is the same object to be stored in.
 336  */
 337 struct dn_ht *
 338 dn_ht_init(struct dn_ht *ht, int buckets, int ofs,
 339         uint32_t (*h)(uintptr_t, int, void *),
 340         int (*match)(void *, uintptr_t, int, void *),
 341         void *(*newh)(uintptr_t, int, void *))
 342 {
 343         int l;
 344
 345         /*
 346          * Notes about rounding bucket size to a power of two.
 347          * Given the original bucket size, we compute the nearest lower and
 348          * higher power of two, minus 1  (respectively b_min and b_max) because
 349          * this value will be used to do an AND with the index returned
 350          * by hash function.
 351          * To choice between these two values, the original bucket size is
 352          * compared with b_min. If the original size is greater than 4/3 b_min,
 353          * we round the bucket size to b_max, else to b_min.
 354          * This ratio try to round to the nearest power of two, advantaging
 355          * the greater size if the different between two power is relatively
 356          * big.
 357          * Rounding the bucket size to a power of two avoid the use of
 358          * module when calculating the correct bucket.
 359          * The ht->buckets variable store the bucket size - 1 to simply
 360          * do an AND between the index returned by hash function and ht->bucket
 361          * instead of a module.
 362          */
 363         int b_min; /* min buckets */
 364         int b_max; /* max buckets */
 365         int b_ori; /* original buckets */
 366
 367         if (h == NULL || match == NULL) {
 368                 printf("--- missing hash or match function");
 369                 return NULL;
 370         }
 371         if (buckets < 1 || buckets > 65536)
 372                 return NULL;
 373
 374         b_ori = buckets;
 375         /* calculate next power of 2, - 1*/
 376         buckets |= buckets >> 1;
 377         buckets |= buckets >> 2;
 378         buckets |= buckets >> 4;
 379         buckets |= buckets >> 8;
 380         buckets |= buckets >> 16;
 381
 382         b_max = buckets; /* Next power */
 383         b_min = buckets >> 1; /* Previous power */
 384
 385         /* Calculate the 'nearest' bucket size */
 386         if (b_min * 4000 / 3000 < b_ori)
 387                 buckets = b_max;
 388         else
 389                 buckets = b_min;
 390
 391         if (ht) {       /* see if we can reuse */
 392                 if (buckets <= ht->buckets) {
 393                         ht->buckets = buckets;
 394                 } else {
 395                         /* free pointers if not allocated inline */
 396                         if (ht->ht != (void *)(ht + 1))
 397                                 free(ht->ht, M_DN_HEAP);
 398                         free(ht, M_DN_HEAP);
 399                         ht = NULL;
 400                 }
 401         }
 402         if (ht == NULL) {
 403                 /* Allocate buckets + 1 entries because buckets is use to
 404                  * do the AND with the index returned by hash function
 405                  */
 406                 l = sizeof(*ht) + (buckets + 1) * sizeof(void **);
 407                 ht = malloc(l, M_DN_HEAP, M_NOWAIT | M_ZERO);
 408         }
 409         if (ht) {
 410                 ht->ht = (void **)(ht + 1);
 411                 ht->buckets = buckets;
 412                 ht->ofs = ofs;
 413                 ht->hash = h;
 414                 ht->match = match;
 415                 ht->newh = newh;
 416         }
 417         return ht;
 418 }
 419
 420 /* dummy callback for dn_ht_free to unlink all */
 421 static int
 422 do_del(void *obj, void *arg)
 423 {
 424         (void)obj;
 425         (void)arg;
 426         return DNHT_SCAN_DEL;
 427 }
 428
 429 void
 430 dn_ht_free(struct dn_ht *ht, int flags)
 431 {
 432         if (ht == NULL)
 433                 return;
 434         if (flags & DNHT_REMOVE) {
 435                 (void)dn_ht_scan(ht, do_del, NULL);
 436         } else {
 437                 if (ht->ht && ht->ht != (void *)(ht + 1))
 438                         free(ht->ht, M_DN_HEAP);
 439                 free(ht, M_DN_HEAP);
 440         }
 441 }
 442
 443 int
 444 dn_ht_entries(struct dn_ht *ht)
 445 {
 446         return ht ? ht->entries : 0;
 447 }
 448
 449 /* lookup and optionally create or delete element */
 450 void *
 451 dn_ht_find(struct dn_ht *ht, uintptr_t key, int flags, void *arg)
 452 {
 453         int i;
 454         void **pp, *p;
 455
 456         if (ht == NULL) /* easy on an empty hash */
 457                 return NULL;
 458         i = (ht->buckets == 1) ? 0 :
 459                 (ht->hash(key, flags, arg) & ht->buckets);
 460
 461         for (pp = &ht->ht[i]; (p = *pp); pp = (void **)((char *)p + ht->ofs)) {
 462                 if (flags & DNHT_MATCH_PTR) {
 463                         if (key == (uintptr_t)p)
 464                                 break;
 465                 } else if (ht->match(p, key, flags, arg)) /* found match */
 466                         break;
 467         }
 468         if (p) {
 469                 if (flags & DNHT_REMOVE) {
 470                         /* link in the next element */
 471                         *pp = *(void **)((char *)p + ht->ofs);
 472                         *(void **)((char *)p + ht->ofs) = NULL;
 473                         ht->entries--;
 474                 }
 475         } else if (flags & DNHT_INSERT) {
 476                 // printf("%s before calling new, bucket %d ofs %d\n",
 477                 //      __FUNCTION__, i, ht->ofs);
 478                 p = ht->newh ? ht->newh(key, flags, arg) : (void *)key;
 479                 // printf("%s newh returns %p\n", __FUNCTION__, p);
 480                 if (p) {
 481                         ht->entries++;
 482                         *(void **)((char *)p + ht->ofs) = ht->ht[i];
 483                         ht->ht[i] = p;
 484                 }
 485         }
 486         return p;
 487 }
 488
 489 /*
 490  * do a scan with the option to delete the object. Extract next before
 491  * running the callback because the element may be destroyed there.
 492  */
 493 int
 494 dn_ht_scan(struct dn_ht *ht, int (*fn)(void *, void *), void *arg)
 495 {
 496         int i, ret, found = 0;
 497         void **curp, *cur, *next;
 498
 499         if (ht == NULL || fn == NULL)
 500                 return 0;
 501         for (i = 0; i <= ht->buckets; i++) {
 502                 curp = &ht->ht[i];
 503                 while ( (cur = *curp) != NULL) {
 504                         next = *(void **)((char *)cur + ht->ofs);
 505                         ret = fn(cur, arg);
 506                         if (ret & DNHT_SCAN_DEL) {
 507                                 found++;
 508                                 ht->entries--;
 509                                 *curp = next;
 510                         } else {
 511                                 curp = (void **)((char *)cur + ht->ofs);
 512                         }
 513                         if (ret & DNHT_SCAN_END)
 514                                 return found;
 515                 }
 516         }
 517         return found;
 518 }
 519
 520 /*
 521  * Similar to dn_ht_scan(), except that the scan is performed only
 522  * in the bucket 'bucket'. The function returns a correct bucket number if
 523  * the original is invalid.
 524  * If the callback returns DNHT_SCAN_END, the function move the ht->ht[i]
 525  * pointer to the last entry processed. Moreover, the bucket number passed
 526  * by caller is decremented, because usually the caller increment it.
 527  */
 528 int
 529 dn_ht_scan_bucket(struct dn_ht *ht, int *bucket, int (*fn)(void *, void *),
 530                  void *arg)
 531 {
 532         int i, ret, found = 0;
 533         void **curp, *cur, *next;
 534
 535         if (ht == NULL || fn == NULL)
 536                 return 0;
 537         if (*bucket > ht->buckets)
 538                 *bucket = 0;
 539         i = *bucket;
 540
 541         curp = &ht->ht[i];
 542         while ( (cur = *curp) != NULL) {
 543                 next = *(void **)((char *)cur + ht->ofs);
 544                 ret = fn(cur, arg);
 545                 if (ret & DNHT_SCAN_DEL) {
 546                         found++;
 547                         ht->entries--;
 548                         *curp = next;
 549                 } else {
 550                         curp = (void **)((char *)cur + ht->ofs);
 551                 }
 552                 if (ret & DNHT_SCAN_END)
 553                         return found;
 554         }
 555         return found;
 556 }