xref: /freebsd/sys/netpfil/ipfw/dn_heap.c (revision e6bfd18d21b225af6a0ed67ceeaf1293b7b9eba5) 
1 /*-
2  * SPDX-License-Identifier: BSD-2-Clause
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 }
557