No empty .Rs/.Re
[netbsd-mini2440.git] / common / lib / libc / gen / rpst.c
blob2f875b3a81a626b3aa53e3bc40c70ac391909ed6
1 /* $NetBSD: rpst.c,v 1.8 2009/05/26 22:38:51 yamt Exp $ */
3 /*-
4 * Copyright (c)2009 YAMAMOTO Takashi,
5 * All rights reserved.
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.
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.
30 * radix priority search tree
32 * described in:
33 * SIAM J. COMPUT.
34 * Vol. 14, No. 2, May 1985
35 * PRIORITY SEARCH TREES
36 * EDWARD M. McCREIGHT
38 * ideas from linux:
39 * - grow tree height on-demand.
40 * - allow duplicated X values. in that case, we act as a heap.
43 #include <sys/cdefs.h>
45 #if defined(_KERNEL)
46 __KERNEL_RCSID(0, "$NetBSD: rpst.c,v 1.8 2009/05/26 22:38:51 yamt Exp $");
47 #include <sys/param.h>
48 #else /* defined(_KERNEL) */
49 __RCSID("$NetBSD: rpst.c,v 1.8 2009/05/26 22:38:51 yamt Exp $");
50 #include <assert.h>
51 #include <stdbool.h>
52 #include <string.h>
53 #if 1
54 #define KASSERT assert
55 #else
56 #define KASSERT(a)
57 #endif
58 #endif /* defined(_KERNEL) */
60 #include <sys/rpst.h>
63 * rpst_init_tree: initialize a tree.
66 void
67 rpst_init_tree(struct rpst_tree *t)
70 t->t_root = NULL;
71 t->t_height = 0;
75 * rpst_height2max: calculate the maximum index which can be handled by
76 * a tree with the given height.
78 * 0 ... 0x0000000000000001
79 * 1 ... 0x0000000000000003
80 * 2 ... 0x0000000000000007
81 * 3 ... 0x000000000000000f
83 * 31 ... 0x00000000ffffffff
85 * 63 ... 0xffffffffffffffff
88 static uint64_t
89 rpst_height2max(unsigned int height)
92 KASSERT(height < 64);
93 if (height == 63) {
94 return UINT64_MAX;
96 return (UINT64_C(1) << (height + 1)) - 1;
100 * rpst_level2mask: calculate the mask for the given level in the tree.
102 * the mask used to index root's children is level 0.
105 static uint64_t
106 rpst_level2mask(const struct rpst_tree *t, unsigned int level)
108 uint64_t mask;
110 if (t->t_height < level) {
111 mask = 0;
112 } else {
113 mask = UINT64_C(1) << (t->t_height - level);
115 return mask;
119 * rpst_startmask: calculate the mask for the start of a search.
120 * (ie. the mask for the top-most bit)
123 static uint64_t
124 rpst_startmask(const struct rpst_tree *t)
126 const uint64_t mask = rpst_level2mask(t, 0);
128 KASSERT((mask | (mask - 1)) == rpst_height2max(t->t_height));
129 return mask;
133 * rpst_update_parents: update n_parent of children
136 static inline void
137 rpst_update_parents(struct rpst_node *n)
139 int i;
141 for (i = 0; i < 2; i++) {
142 if (n->n_children[i] != NULL) {
143 n->n_children[i]->n_parent = n;
149 * rpst_enlarge_tree: enlarge tree so that 'idx' can be stored
152 static void
153 rpst_enlarge_tree(struct rpst_tree *t, uint64_t idx)
156 while (idx > rpst_height2max(t->t_height)) {
157 struct rpst_node *n = t->t_root;
159 if (n != NULL) {
160 rpst_remove_node(t, n);
161 memset(&n->n_children, 0, sizeof(n->n_children));
162 n->n_children[0] = t->t_root;
163 t->t_root->n_parent = n;
164 t->t_root = n;
165 n->n_parent = NULL;
167 t->t_height++;
172 * rpst_insert_node1: a helper for rpst_insert_node.
175 static struct rpst_node *
176 rpst_insert_node1(struct rpst_node **where, struct rpst_node *n, uint64_t mask)
178 struct rpst_node *parent;
179 struct rpst_node *cur;
180 unsigned int idx;
182 KASSERT((n->n_x & ((-mask) << 1)) == 0);
183 parent = NULL;
184 next:
185 cur = *where;
186 if (cur == NULL) {
187 n->n_parent = parent;
188 memset(&n->n_children, 0, sizeof(n->n_children));
189 *where = n;
190 return NULL;
192 KASSERT(cur->n_parent == parent);
193 if (n->n_y == cur->n_y && n->n_x == cur->n_x) {
194 return cur;
196 if (n->n_y < cur->n_y) {
198 * swap cur and n.
199 * note that n is not in tree.
201 memcpy(n->n_children, cur->n_children, sizeof(n->n_children));
202 n->n_parent = cur->n_parent;
203 rpst_update_parents(n);
204 *where = n;
205 n = cur;
206 cur = *where;
208 KASSERT(*where == cur);
209 idx = (n->n_x & mask) != 0;
210 where = &cur->n_children[idx];
211 parent = cur;
212 KASSERT((*where) == NULL || ((((*where)->n_x & mask) != 0) == idx));
213 KASSERT((*where) == NULL || (*where)->n_y >= cur->n_y);
214 mask >>= 1;
215 goto next;
219 * rpst_insert_node: insert a node into the tree.
221 * => return NULL on success.
222 * => if a duplicated node (a node with the same X,Y pair as ours) is found,
223 * return the node. in that case, the tree is intact.
226 struct rpst_node *
227 rpst_insert_node(struct rpst_tree *t, struct rpst_node *n)
230 rpst_enlarge_tree(t, n->n_x);
231 return rpst_insert_node1(&t->t_root, n, rpst_startmask(t));
235 * rpst_find_pptr: find a pointer to the given node.
237 * also, return the parent node via parentp. (NULL for the root node.)
240 static inline struct rpst_node **
241 rpst_find_pptr(struct rpst_tree *t, struct rpst_node *n,
242 struct rpst_node **parentp)
244 struct rpst_node * const parent = n->n_parent;
245 unsigned int i;
247 *parentp = parent;
248 if (parent == NULL) {
249 return &t->t_root;
251 for (i = 0; i < 2 - 1; i++) {
252 if (parent->n_children[i] == n) {
253 break;
256 KASSERT(parent->n_children[i] == n);
257 return &parent->n_children[i];
261 * rpst_remove_node_at: remove a node at *where.
264 static void
265 rpst_remove_node_at(struct rpst_node *parent, struct rpst_node **where,
266 struct rpst_node *cur)
268 struct rpst_node *tmp[2];
269 struct rpst_node *selected;
270 unsigned int selected_idx = 0; /* XXX gcc */
271 unsigned int i;
273 KASSERT(cur != NULL);
274 KASSERT(parent == cur->n_parent);
275 next:
276 selected = NULL;
277 for (i = 0; i < 2; i++) {
278 struct rpst_node *c;
280 c = cur->n_children[i];
281 KASSERT(c == NULL || c->n_parent == cur);
282 if (selected == NULL || (c != NULL && c->n_y < selected->n_y)) {
283 selected = c;
284 selected_idx = i;
288 * now we have:
290 * parent
291 * \ <- where
292 * cur
293 * / \
294 * A selected
295 * / \
296 * B C
298 *where = selected;
299 if (selected == NULL) {
300 return;
303 * swap selected->n_children and cur->n_children.
305 memcpy(tmp, selected->n_children, sizeof(tmp));
306 memcpy(selected->n_children, cur->n_children, sizeof(tmp));
307 memcpy(cur->n_children, tmp, sizeof(tmp));
308 rpst_update_parents(cur);
309 rpst_update_parents(selected);
310 selected->n_parent = parent;
312 * parent
313 * \ <- where
314 * selected
315 * / \
316 * A selected
318 * cur
319 * / \
320 * B C
322 where = &selected->n_children[selected_idx];
324 * parent
326 * selected
327 * / \ <- where
328 * A selected (*)
330 * cur (**)
331 * / \
332 * B C
334 * (*) this 'selected' will be overwritten in the next iteration.
335 * (**) cur->n_parent is bogus.
337 parent = selected;
338 goto next;
342 * rpst_remove_node: remove a node from the tree.
345 void
346 rpst_remove_node(struct rpst_tree *t, struct rpst_node *n)
348 struct rpst_node *parent;
349 struct rpst_node **where;
351 where = rpst_find_pptr(t, n, &parent);
352 rpst_remove_node_at(parent, where, n);
355 static bool __unused
356 rpst_iterator_match_p(const struct rpst_node *n, const struct rpst_iterator *it)
359 if (n->n_y > it->it_max_y) {
360 return false;
362 if (n->n_x < it->it_min_x) {
363 return false;
365 if (n->n_x > it->it_max_x) {
366 return false;
368 return true;
371 struct rpst_node *
372 rpst_iterate_first(struct rpst_tree *t, uint64_t max_y, uint64_t min_x,
373 uint64_t max_x, struct rpst_iterator *it)
375 struct rpst_node *n;
377 KASSERT(min_x <= max_x);
378 n = t->t_root;
379 if (n == NULL || n->n_y > max_y) {
380 return NULL;
382 if (rpst_height2max(t->t_height) < min_x) {
383 return NULL;
385 it->it_tree = t;
386 it->it_cur = n;
387 it->it_idx = (min_x & rpst_startmask(t)) != 0;
388 it->it_level = 0;
389 it->it_max_y = max_y;
390 it->it_min_x = min_x;
391 it->it_max_x = max_x;
392 return rpst_iterate_next(it);
395 static inline unsigned int
396 rpst_node_on_edge_p(const struct rpst_node *n, uint64_t val, uint64_t mask)
399 return ((n->n_x ^ val) & ((-mask) << 1)) == 0;
402 static inline uint64_t
403 rpst_maxidx(const struct rpst_node *n, uint64_t max_x, uint64_t mask)
406 if (rpst_node_on_edge_p(n, max_x, mask)) {
407 return (max_x & mask) != 0;
408 } else {
409 return 1;
413 static inline uint64_t
414 rpst_minidx(const struct rpst_node *n, uint64_t min_x, uint64_t mask)
417 if (rpst_node_on_edge_p(n, min_x, mask)) {
418 return (min_x & mask) != 0;
419 } else {
420 return 0;
424 struct rpst_node *
425 rpst_iterate_next(struct rpst_iterator *it)
427 struct rpst_tree *t;
428 struct rpst_node *n;
429 struct rpst_node *next;
430 const uint64_t max_y = it->it_max_y;
431 const uint64_t min_x = it->it_min_x;
432 const uint64_t max_x = it->it_max_x;
433 unsigned int idx;
434 unsigned int maxidx;
435 unsigned int level;
436 uint64_t mask;
438 t = it->it_tree;
439 n = it->it_cur;
440 idx = it->it_idx;
441 level = it->it_level;
442 mask = rpst_level2mask(t, level);
443 maxidx = rpst_maxidx(n, max_x, mask);
444 KASSERT(n == t->t_root || rpst_iterator_match_p(n, it));
445 next:
446 KASSERT(mask == rpst_level2mask(t, level));
447 KASSERT(idx >= rpst_minidx(n, min_x, mask));
448 KASSERT(maxidx == rpst_maxidx(n, max_x, mask));
449 KASSERT(idx <= maxidx + 2);
450 KASSERT(n != NULL);
451 #if 0
452 printf("%s: cur=%p, idx=%u maxidx=%u level=%u mask=%" PRIx64 "\n",
453 __func__, (void *)n, idx, maxidx, level, mask);
454 #endif
455 if (idx == maxidx + 1) { /* visit the current node */
456 idx++;
457 if (min_x <= n->n_x && n->n_x <= max_x) {
458 it->it_cur = n;
459 it->it_idx = idx;
460 it->it_level = level;
461 KASSERT(rpst_iterator_match_p(n, it));
462 return n; /* report */
464 goto next;
465 } else if (idx == maxidx + 2) { /* back to the parent */
466 struct rpst_node **where;
468 where = rpst_find_pptr(t, n, &next);
469 if (next == NULL) {
470 KASSERT(level == 0);
471 KASSERT(t->t_root == n);
472 KASSERT(&t->t_root == where);
473 return NULL; /* done */
475 KASSERT(level > 0);
476 level--;
477 n = next;
478 mask = rpst_level2mask(t, level);
479 maxidx = rpst_maxidx(n, max_x, mask);
480 idx = where - n->n_children + 1;
481 KASSERT(idx < 2 + 1);
482 goto next;
484 /* go to a child */
485 KASSERT(idx < 2);
486 next = n->n_children[idx];
487 if (next == NULL || next->n_y > max_y) {
488 idx++;
489 goto next;
491 KASSERT(next->n_parent == n);
492 KASSERT(next->n_y >= n->n_y);
493 level++;
494 mask >>= 1;
495 n = next;
496 idx = rpst_minidx(n, min_x, mask);
497 maxidx = rpst_maxidx(n, max_x, mask);
498 #if 0
499 printf("%s: visit %p idx=%u level=%u mask=%llx\n",
500 __func__, n, idx, level, mask);
501 #endif
502 goto next;
505 #if defined(UNITTEST)
506 #include <sys/time.h>
508 #include <inttypes.h>
509 #include <stdio.h>
510 #include <stdlib.h>
512 static void
513 rpst_dump_node(const struct rpst_node *n, unsigned int depth)
515 unsigned int i;
517 for (i = 0; i < depth; i++) {
518 printf(" ");
520 printf("[%u]", depth);
521 if (n == NULL) {
522 printf("NULL\n");
523 return;
525 printf("%p x=%" PRIx64 "(%" PRIu64 ") y=%" PRIx64 "(%" PRIu64 ")\n",
526 (const void *)n, n->n_x, n->n_x, n->n_y, n->n_y);
527 for (i = 0; i < 2; i++) {
528 rpst_dump_node(n->n_children[i], depth + 1);
532 static void
533 rpst_dump_tree(const struct rpst_tree *t)
536 printf("pst %p height=%u\n", (const void *)t, t->t_height);
537 rpst_dump_node(t->t_root, 0);
540 struct testnode {
541 struct rpst_node n;
542 struct testnode *next;
543 bool failed;
544 bool found;
547 struct rpst_tree t;
548 struct testnode *h = NULL;
550 static uintmax_t
551 tvdiff(const struct timeval *tv1, const struct timeval *tv2)
554 return (uintmax_t)tv1->tv_sec * 1000000 + tv1->tv_usec -
555 tv2->tv_sec * 1000000 - tv2->tv_usec;
558 static unsigned int
559 query(uint64_t max_y, uint64_t min_x, uint64_t max_x)
561 struct testnode *n;
562 struct rpst_node *rn;
563 struct rpst_iterator it;
564 struct timeval start;
565 struct timeval end;
566 unsigned int done;
568 printf("quering max_y=%" PRIu64 " min_x=%" PRIu64 " max_x=%" PRIu64
569 "\n",
570 max_y, min_x, max_x);
571 done = 0;
572 gettimeofday(&start, NULL);
573 for (rn = rpst_iterate_first(&t, max_y, min_x, max_x, &it);
574 rn != NULL;
575 rn = rpst_iterate_next(&it)) {
576 done++;
577 #if 0
578 printf("found %p x=%" PRIu64 " y=%" PRIu64 "\n",
579 (void *)rn, rn->n_x, rn->n_y);
580 #endif
581 n = (void *)rn;
582 assert(!n->found);
583 n->found = true;
585 gettimeofday(&end, NULL);
586 printf("%u nodes found in %ju usecs\n", done,
587 tvdiff(&end, &start));
589 gettimeofday(&start, NULL);
590 for (n = h; n != NULL; n = n->next) {
591 assert(n->failed ||
592 n->found == rpst_iterator_match_p(&n->n, &it));
593 n->found = false;
595 gettimeofday(&end, NULL);
596 printf("(linear search took %ju usecs)\n", tvdiff(&end, &start));
597 return done;
601 main(int argc, char *argv[])
603 struct testnode *n;
604 unsigned int i;
605 struct rpst_iterator it;
606 struct timeval start;
607 struct timeval end;
608 uint64_t min_y = UINT64_MAX;
609 uint64_t max_y = 0;
610 uint64_t min_x = UINT64_MAX;
611 uint64_t max_x = 0;
612 uint64_t w;
613 unsigned int done;
614 unsigned int fail;
615 unsigned int num = 500000;
617 rpst_init_tree(&t);
618 rpst_dump_tree(&t);
619 assert(NULL == rpst_iterate_first(&t, UINT64_MAX, 0, UINT64_MAX, &it));
621 for (i = 0; i < num; i++) {
622 n = malloc(sizeof(*n));
623 if (i > 499000) {
624 n->n.n_x = 10;
625 n->n.n_y = random();
626 } else if (i > 400000) {
627 n->n.n_x = i;
628 n->n.n_y = random();
629 } else {
630 n->n.n_x = random();
631 n->n.n_y = random();
633 if (n->n.n_y < min_y) {
634 min_y = n->n.n_y;
636 if (n->n.n_y > max_y) {
637 max_y = n->n.n_y;
639 if (n->n.n_x < min_x) {
640 min_x = n->n.n_x;
642 if (n->n.n_x > max_x) {
643 max_x = n->n.n_x;
645 n->found = false;
646 n->failed = false;
647 n->next = h;
648 h = n;
651 done = 0;
652 fail = 0;
653 gettimeofday(&start, NULL);
654 for (n = h; n != NULL; n = n->next) {
655 struct rpst_node *o;
656 #if 0
657 printf("insert %p x=%" PRIu64 " y=%" PRIu64 "\n",
658 n, n->n.n_x, n->n.n_y);
659 #endif
660 o = rpst_insert_node(&t, &n->n);
661 if (o == NULL) {
662 done++;
663 } else {
664 n->failed = true;
665 fail++;
668 gettimeofday(&end, NULL);
669 printf("%u nodes inserted and %u insertion failed in %ju usecs\n",
670 done, fail,
671 tvdiff(&end, &start));
673 assert(min_y == 0 || 0 == query(min_y - 1, 0, UINT64_MAX));
674 assert(max_x == UINT64_MAX ||
675 0 == query(UINT64_MAX, max_x + 1, UINT64_MAX));
676 assert(min_x == 0 || 0 == query(UINT64_MAX, 0, min_x - 1));
678 done = query(max_y, min_x, max_x);
679 assert(done == num - fail);
681 done = query(UINT64_MAX, 0, UINT64_MAX);
682 assert(done == num - fail);
684 w = max_x - min_x;
685 query(max_y / 2, min_x, max_x);
686 query(max_y, min_x + w / 2, max_x);
687 query(max_y / 2, min_x + w / 2, max_x);
688 query(max_y / 2, min_x, max_x - w / 2);
689 query(max_y / 2, min_x + w / 3, max_x - w / 3);
690 query(max_y - 1, min_x + 1, max_x - 1);
691 query(UINT64_MAX, 10, 10);
693 done = 0;
694 gettimeofday(&start, NULL);
695 for (n = h; n != NULL; n = n->next) {
696 if (n->failed) {
697 continue;
699 #if 0
700 printf("remove %p x=%" PRIu64 " y=%" PRIu64 "\n",
701 n, n->n.n_x, n->n.n_y);
702 #endif
703 rpst_remove_node(&t, &n->n);
704 done++;
706 gettimeofday(&end, NULL);
707 printf("%u nodes removed in %ju usecs\n", done,
708 tvdiff(&end, &start));
710 rpst_dump_tree(&t);
712 #endif /* defined(UNITTEST) */