1 /* $NetBSD: rpst.c,v 1.9 2009/05/26 22:39:15 yamt Exp $ */ 2 3 /*- 4 * Copyright (c)2009 YAMAMOTO Takashi, 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 * radix priority search tree 31 * 32 * described in: 33 * SIAM J. COMPUT. 34 * Vol. 14, No. 2, May 1985 35 * PRIORITY SEARCH TREES 36 * EDWARD M. McCREIGHT 37 * 38 * ideas from linux: 39 * - grow tree height on-demand. 40 * - allow duplicated X values. in that case, we act as a heap. 41 */ 42 43 #include <sys/cdefs.h> 44 45 #if defined(_KERNEL) 46 __KERNEL_RCSID(0, "$NetBSD: rpst.c,v 1.9 2009/05/26 22:39:15 yamt Exp $"); 47 #include <sys/param.h> 48 #else /* defined(_KERNEL) */ 49 __RCSID("$NetBSD: rpst.c,v 1.9 2009/05/26 22:39:15 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) */ 59 60 #include <sys/rpst.h> 61 62 /* 63 * rpst_init_tree: initialize a tree. 64 */ 65 66 void 67 rpst_init_tree(struct rpst_tree *t) 68 { 69 70 t->t_root = NULL; 71 t->t_height = 0; 72 } 73 74 /* 75 * rpst_height2max: calculate the maximum index which can be handled by 76 * a tree with the given height. 77 * 78 * 0 ... 0x0000000000000001 79 * 1 ... 0x0000000000000003 80 * 2 ... 0x0000000000000007 81 * 3 ... 0x000000000000000f 82 * 83 * 31 ... 0x00000000ffffffff 84 * 85 * 63 ... 0xffffffffffffffff 86 */ 87 88 static uint64_t 89 rpst_height2max(unsigned int height) 90 { 91 92 KASSERT(height < 64); 93 if (height == 63) { 94 return UINT64_MAX; 95 } 96 return (UINT64_C(1) << (height + 1)) - 1; 97 } 98 99 /* 100 * rpst_level2mask: calculate the mask for the given level in the tree. 101 * 102 * the mask used to index root's children is level 0. 103 */ 104 105 static uint64_t 106 rpst_level2mask(const struct rpst_tree *t, unsigned int level) 107 { 108 uint64_t mask; 109 110 if (t->t_height < level) { 111 mask = 0; 112 } else { 113 mask = UINT64_C(1) << (t->t_height - level); 114 } 115 return mask; 116 } 117 118 /* 119 * rpst_startmask: calculate the mask for the start of a search. 120 * (ie. the mask for the top-most bit) 121 */ 122 123 static uint64_t 124 rpst_startmask(const struct rpst_tree *t) 125 { 126 const uint64_t mask = rpst_level2mask(t, 0); 127 128 KASSERT((mask | (mask - 1)) == rpst_height2max(t->t_height)); 129 return mask; 130 } 131 132 /* 133 * rpst_update_parents: update n_parent of children 134 */ 135 136 static inline void 137 rpst_update_parents(struct rpst_node *n) 138 { 139 int i; 140 141 for (i = 0; i < 2; i++) { 142 if (n->n_children[i] != NULL) { 143 n->n_children[i]->n_parent = n; 144 } 145 } 146 } 147 148 /* 149 * rpst_enlarge_tree: enlarge tree so that 'idx' can be stored 150 */ 151 152 static void 153 rpst_enlarge_tree(struct rpst_tree *t, uint64_t idx) 154 { 155 156 while (idx > rpst_height2max(t->t_height)) { 157 struct rpst_node *n = t->t_root; 158 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; 166 } 167 t->t_height++; 168 } 169 } 170 171 /* 172 * rpst_insert_node1: a helper for rpst_insert_node. 173 */ 174 175 static struct rpst_node * 176 rpst_insert_node1(struct rpst_node **where, struct rpst_node *n, uint64_t mask) 177 { 178 struct rpst_node *parent; 179 struct rpst_node *cur; 180 unsigned int idx; 181 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; 191 } 192 KASSERT(cur->n_parent == parent); 193 if (n->n_y == cur->n_y && n->n_x == cur->n_x) { 194 return cur; 195 } 196 if (n->n_y < cur->n_y) { 197 /* 198 * swap cur and n. 199 * note that n is not in tree. 200 */ 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; 207 } 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; 216 } 217 218 /* 219 * rpst_insert_node: insert a node into the tree. 220 * 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. 224 */ 225 226 struct rpst_node * 227 rpst_insert_node(struct rpst_tree *t, struct rpst_node *n) 228 { 229 230 rpst_enlarge_tree(t, n->n_x); 231 return rpst_insert_node1(&t->t_root, n, rpst_startmask(t)); 232 } 233 234 /* 235 * rpst_find_pptr: find a pointer to the given node. 236 * 237 * also, return the parent node via parentp. (NULL for the root node.) 238 */ 239 240 static inline struct rpst_node ** 241 rpst_find_pptr(struct rpst_tree *t, struct rpst_node *n, 242 struct rpst_node **parentp) 243 { 244 struct rpst_node * const parent = n->n_parent; 245 unsigned int i; 246 247 *parentp = parent; 248 if (parent == NULL) { 249 return &t->t_root; 250 } 251 for (i = 0; i < 2 - 1; i++) { 252 if (parent->n_children[i] == n) { 253 break; 254 } 255 } 256 KASSERT(parent->n_children[i] == n); 257 return &parent->n_children[i]; 258 } 259 260 /* 261 * rpst_remove_node_at: remove a node at *where. 262 */ 263 264 static void 265 rpst_remove_node_at(struct rpst_node *parent, struct rpst_node **where, 266 struct rpst_node *cur) 267 { 268 struct rpst_node *tmp[2]; 269 struct rpst_node *selected; 270 unsigned int selected_idx = 0; /* XXX gcc */ 271 unsigned int i; 272 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; 279 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; 285 } 286 } 287 /* 288 * now we have: 289 * 290 * parent 291 * \ <- where 292 * cur 293 * / \ 294 * A selected 295 * / \ 296 * B C 297 */ 298 *where = selected; 299 if (selected == NULL) { 300 return; 301 } 302 /* 303 * swap selected->n_children and cur->n_children. 304 */ 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; 311 /* 312 * parent 313 * \ <- where 314 * selected 315 * / \ 316 * A selected 317 * 318 * cur 319 * / \ 320 * B C 321 */ 322 where = &selected->n_children[selected_idx]; 323 /* 324 * parent 325 * \ 326 * selected 327 * / \ <- where 328 * A selected (*) 329 * 330 * cur (**) 331 * / \ 332 * B C 333 * 334 * (*) this 'selected' will be overwritten in the next iteration. 335 * (**) cur->n_parent is bogus. 336 */ 337 parent = selected; 338 goto next; 339 } 340 341 /* 342 * rpst_remove_node: remove a node from the tree. 343 */ 344 345 void 346 rpst_remove_node(struct rpst_tree *t, struct rpst_node *n) 347 { 348 struct rpst_node *parent; 349 struct rpst_node **where; 350 351 where = rpst_find_pptr(t, n, &parent); 352 rpst_remove_node_at(parent, where, n); 353 } 354 355 static bool __unused 356 rpst_iterator_match_p(const struct rpst_node *n, const struct rpst_iterator *it) 357 { 358 359 if (n->n_y > it->it_max_y) { 360 return false; 361 } 362 if (n->n_x < it->it_min_x) { 363 return false; 364 } 365 if (n->n_x > it->it_max_x) { 366 return false; 367 } 368 return true; 369 } 370 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) 374 { 375 struct rpst_node *n; 376 377 KASSERT(min_x <= max_x); 378 n = t->t_root; 379 if (n == NULL || n->n_y > max_y) { 380 return NULL; 381 } 382 if (rpst_height2max(t->t_height) < min_x) { 383 return NULL; 384 } 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); 393 } 394 395 static inline unsigned int 396 rpst_node_on_edge_p(const struct rpst_node *n, uint64_t val, uint64_t mask) 397 { 398 399 return ((n->n_x ^ val) & ((-mask) << 1)) == 0; 400 } 401 402 static inline uint64_t 403 rpst_maxidx(const struct rpst_node *n, uint64_t max_x, uint64_t mask) 404 { 405 406 if (rpst_node_on_edge_p(n, max_x, mask)) { 407 return (max_x & mask) != 0; 408 } else { 409 return 1; 410 } 411 } 412 413 static inline uint64_t 414 rpst_minidx(const struct rpst_node *n, uint64_t min_x, uint64_t mask) 415 { 416 417 if (rpst_node_on_edge_p(n, min_x, mask)) { 418 return (min_x & mask) != 0; 419 } else { 420 return 0; 421 } 422 } 423 424 struct rpst_node * 425 rpst_iterate_next(struct rpst_iterator *it) 426 { 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; 437 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 */ 463 } 464 goto next; 465 } else if (idx == maxidx + 2) { /* back to the parent */ 466 struct rpst_node **where; 467 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 */ 474 } 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; 483 } 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; 490 } 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; 503 } 504 505 #if defined(UNITTEST) 506 #include <sys/time.h> 507 508 #include <inttypes.h> 509 #include <stdio.h> 510 #include <stdlib.h> 511 512 static void 513 rpst_dump_node(const struct rpst_node *n, unsigned int depth) 514 { 515 unsigned int i; 516 517 for (i = 0; i < depth; i++) { 518 printf(" "); 519 } 520 printf("[%u]", depth); 521 if (n == NULL) { 522 printf("NULL\n"); 523 return; 524 } 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); 529 } 530 } 531 532 static void 533 rpst_dump_tree(const struct rpst_tree *t) 534 { 535 536 printf("pst %p height=%u\n", (const void *)t, t->t_height); 537 rpst_dump_node(t->t_root, 0); 538 } 539 540 struct testnode { 541 struct rpst_node n; 542 struct testnode *next; 543 bool failed; 544 bool found; 545 }; 546 547 struct rpst_tree t; 548 struct testnode *h = NULL; 549 550 static uintmax_t 551 tvdiff(const struct timeval *tv1, const struct timeval *tv2) 552 { 553 554 return (uintmax_t)tv1->tv_sec * 1000000 + tv1->tv_usec - 555 tv2->tv_sec * 1000000 - tv2->tv_usec; 556 } 557 558 static unsigned int 559 query(uint64_t max_y, uint64_t min_x, uint64_t max_x) 560 { 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; 567 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; 584 } 585 gettimeofday(&end, NULL); 586 printf("%u nodes found in %ju usecs\n", done, 587 tvdiff(&end, &start)); 588 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; 594 } 595 gettimeofday(&end, NULL); 596 printf("(linear search took %ju usecs)\n", tvdiff(&end, &start)); 597 return done; 598 } 599 600 int 601 main(int argc, char *argv[]) 602 { 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; 616 617 rpst_init_tree(&t); 618 rpst_dump_tree(&t); 619 assert(NULL == rpst_iterate_first(&t, UINT64_MAX, 0, UINT64_MAX, &it)); 620 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(); 632 } 633 if (n->n.n_y < min_y) { 634 min_y = n->n.n_y; 635 } 636 if (n->n.n_y > max_y) { 637 max_y = n->n.n_y; 638 } 639 if (n->n.n_x < min_x) { 640 min_x = n->n.n_x; 641 } 642 if (n->n.n_x > max_x) { 643 max_x = n->n.n_x; 644 } 645 n->found = false; 646 n->failed = false; 647 n->next = h; 648 h = n; 649 } 650 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++; 666 } 667 } 668 gettimeofday(&end, NULL); 669 printf("%u nodes inserted and %u insertion failed in %ju usecs\n", 670 done, fail, 671 tvdiff(&end, &start)); 672 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)); 677 678 done = query(max_y, min_x, max_x); 679 assert(done == num - fail); 680 681 done = query(UINT64_MAX, 0, UINT64_MAX); 682 assert(done == num - fail); 683 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); 692 693 done = 0; 694 gettimeofday(&start, NULL); 695 for (n = h; n != NULL; n = n->next) { 696 if (n->failed) { 697 continue; 698 } 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++; 705 } 706 gettimeofday(&end, NULL); 707 printf("%u nodes removed in %ju usecs\n", done, 708 tvdiff(&end, &start)); 709 710 rpst_dump_tree(&t); 711 } 712 #endif /* defined(UNITTEST) */ 713