2 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
4 * Copyright (c) 1998-2002,2010 Luigi Rizzo, Universita` di Pisa
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
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.
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
30 * Binary heap and hash tables, used in dummynet
35 #include <sys/cdefs.h>
36 #include <sys/param.h>
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>
44 #define log(x, arg...)
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)
64 static MALLOC_DEFINE(M_DN_HEAP, "dummynet", "dummynet heap");
67 * Heap management functions.
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.
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
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
82 heap_resize(struct dn_heap *h, unsigned int new_size)
84 struct dn_heap_entry *p;
86 if ((unsigned int)h->size >= new_size ) /* have enough room */
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;
95 new_size = (new_size + HEAP_INCREMENT ) & ~HEAP_INCREMENT;
97 p = malloc(new_size * sizeof(*p), M_DN_HEAP, M_NOWAIT);
99 printf("--- %s, resize %d failed\n", __func__, new_size );
100 return 1; /* error */
103 bcopy(h->p, p, h->size * sizeof(*p) );
104 free(h->p, M_DN_HEAP);
112 heap_init(struct dn_heap *h, int size, int ofs)
114 if (heap_resize(h, size))
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
126 * Returns 1 on failure (cannot allocate new heap entry)
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.
131 #define SET_OFFSET(h, i) do { \
133 *((int32_t *)((char *)(h->p[i].object) + h->ofs)) = i; \
136 * RESET_OFFSET is used for sanity checks. It sets ofs
137 * to an invalid value.
139 #define RESET_OFFSET(h, i) do { \
141 *((int32_t *)((char *)(h->p[i].object) + h->ofs)) = -16; \
145 heap_insert(struct dn_heap *h, uint64_t key1, void *p)
147 int son = h->elements;
149 //log("%s key %llu p %p\n", __FUNCTION__, key1, p);
150 if (p == NULL) { /* data already there, set starting point */
152 } else { /* insert new element at the end, possibly resize */
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;
162 /* make sure that son >= father along the path */
164 int father = HEAP_FATHER(son);
165 struct dn_heap_entry tmp;
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);
179 * remove top element from heap, or obj if obj != NULL
182 heap_extract(struct dn_heap *h, void *obj)
184 int child, father, max = h->elements - 1;
187 printf("--- %s: empty heap 0x%p\n", __FUNCTION__, h);
191 father = 0; /* default: move up smallest child */
192 else { /* extract specific element, index is at offset */
194 panic("%s: extract from middle not set on %p\n",
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);
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.
207 // XXX why removing RESET_OFFSET increases runtime by 10% ?
208 RESET_OFFSET(h, father);
209 while ( (child = HEAP_LEFT(father)) <= 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);
220 * Fill hole with last entry and bubble up,
221 * reusing the insert code
223 h->p[father] = h->p[max];
224 heap_insert(h, father, NULL);
230 * change object position and update references
231 * XXX this one is never used!
234 heap_move(struct dn_heap *h, uint64_t new_key, void *object)
236 int temp, i, max = h->elements-1;
237 struct dn_heap_entry *p, buf;
240 panic("cannot move items on this heap");
241 p = h->p; /* shortcut */
243 i = *((int *)((char *)object + h->ofs));
244 if (DN_KEY_LT(new_key, p[i].key) ) { /* must move up */
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);
252 } else { /* must move down */
254 while ( (temp = HEAP_LEFT(i)) <= max ) {
255 /* found left child */
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)) {
261 HEAP_SWAP(p[i], p[temp], buf);
270 #endif /* heap_move, unused */
273 * heapify() will reorganize data inside an array to maintain the
274 * heap property. It is needed when we delete a bunch of entries.
277 heapify(struct dn_heap *h)
281 for (i = 0; i < h->elements; i++ )
282 heap_insert(h, i , NULL);
286 heap_scan(struct dn_heap *h, int (*fn)(void *, uintptr_t),
291 for (i = found = 0 ; i < h->elements ;) {
292 ret = fn(h->p[i].object, arg);
293 if (ret & HEAP_SCAN_DEL) {
295 h->p[i] = h->p[h->elements] ;
299 if (ret & HEAP_SCAN_END)
308 * cleanup the heap and free data structure
311 heap_free(struct dn_heap *h)
314 free(h->p, M_DN_HEAP);
315 bzero(h, sizeof(*h) );
319 * hash table support.
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 */
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.
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 *))
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
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
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.
363 int b_min; /* min buckets */
364 int b_max; /* max buckets */
365 int b_ori; /* original buckets */
367 if (h == NULL || match == NULL) {
368 printf("--- missing hash or match function");
371 if (buckets < 1 || buckets > 65536)
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;
382 b_max = buckets; /* Next power */
383 b_min = buckets >> 1; /* Previous power */
385 /* Calculate the 'nearest' bucket size */
386 if (b_min * 4000 / 3000 < b_ori)
391 if (ht) { /* see if we can reuse */
392 if (buckets <= ht->buckets) {
393 ht->buckets = buckets;
395 /* free pointers if not allocated inline */
396 if (ht->ht != (void *)(ht + 1))
397 free(ht->ht, M_DN_HEAP);
403 /* Allocate buckets + 1 entries because buckets is use to
404 * do the AND with the index returned by hash function
406 l = sizeof(*ht) + (buckets + 1) * sizeof(void **);
407 ht = malloc(l, M_DN_HEAP, M_NOWAIT | M_ZERO);
410 ht->ht = (void **)(ht + 1);
411 ht->buckets = buckets;
420 /* dummy callback for dn_ht_free to unlink all */
422 do_del(void *obj, void *arg)
426 return DNHT_SCAN_DEL;
430 dn_ht_free(struct dn_ht *ht, int flags)
434 if (flags & DNHT_REMOVE) {
435 (void)dn_ht_scan(ht, do_del, NULL);
437 if (ht->ht && ht->ht != (void *)(ht + 1))
438 free(ht->ht, M_DN_HEAP);
444 dn_ht_entries(struct dn_ht *ht)
446 return ht ? ht->entries : 0;
449 /* lookup and optionally create or delete element */
451 dn_ht_find(struct dn_ht *ht, uintptr_t key, int flags, void *arg)
456 if (ht == NULL) /* easy on an empty hash */
458 i = (ht->buckets == 1) ? 0 :
459 (ht->hash(key, flags, arg) & ht->buckets);
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)
465 } else if (ht->match(p, key, flags, arg)) /* found match */
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;
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);
482 *(void **)((char *)p + ht->ofs) = ht->ht[i];
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.
494 dn_ht_scan(struct dn_ht *ht, int (*fn)(void *, void *), void *arg)
496 int i, ret, found = 0;
497 void **curp, *cur, *next;
499 if (ht == NULL || fn == NULL)
501 for (i = 0; i <= ht->buckets; i++) {
503 while ( (cur = *curp) != NULL) {
504 next = *(void **)((char *)cur + ht->ofs);
506 if (ret & DNHT_SCAN_DEL) {
511 curp = (void **)((char *)cur + ht->ofs);
513 if (ret & DNHT_SCAN_END)
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.
529 dn_ht_scan_bucket(struct dn_ht *ht, int *bucket, int (*fn)(void *, void *),
532 int i, ret, found = 0;
533 void **curp, *cur, *next;
535 if (ht == NULL || fn == NULL)
537 if (*bucket > ht->buckets)
542 while ( (cur = *curp) != NULL) {
543 next = *(void **)((char *)cur + ht->ofs);
545 if (ret & DNHT_SCAN_DEL) {
550 curp = (void **)((char *)cur + ht->ofs);
552 if (ret & DNHT_SCAN_END)