1 /* $NetBSD: ip_encap.c,v 1.44 2015/04/16 06:50:16 ozaki-r Exp $ */ 2 /* $KAME: ip_encap.c,v 1.73 2001/10/02 08:30:58 itojun Exp $ */ 3 4 /* 5 * Copyright (C) 1995, 1996, 1997, and 1998 WIDE Project. 6 * All rights reserved. 7 * 8 * Redistribution and use in source and binary forms, with or without 9 * modification, are permitted provided that the following conditions 10 * are met: 11 * 1. Redistributions of source code must retain the above copyright 12 * notice, this list of conditions and the following disclaimer. 13 * 2. Redistributions in binary form must reproduce the above copyright 14 * notice, this list of conditions and the following disclaimer in the 15 * documentation and/or other materials provided with the distribution. 16 * 3. Neither the name of the project nor the names of its contributors 17 * may be used to endorse or promote products derived from this software 18 * without specific prior written permission. 19 * 20 * THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND 21 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 * ARE DISCLAIMED. IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE 24 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 26 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 27 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 29 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 30 * SUCH DAMAGE. 31 */ 32 /* 33 * My grandfather said that there's a devil inside tunnelling technology... 34 * 35 * We have surprisingly many protocols that want packets with IP protocol 36 * #4 or #41. Here's a list of protocols that want protocol #41: 37 * RFC1933 configured tunnel 38 * RFC1933 automatic tunnel 39 * RFC2401 IPsec tunnel 40 * RFC2473 IPv6 generic packet tunnelling 41 * RFC2529 6over4 tunnel 42 * RFC3056 6to4 tunnel 43 * isatap tunnel 44 * mobile-ip6 (uses RFC2473) 45 * Here's a list of protocol that want protocol #4: 46 * RFC1853 IPv4-in-IPv4 tunnelling 47 * RFC2003 IPv4 encapsulation within IPv4 48 * RFC2344 reverse tunnelling for mobile-ip4 49 * RFC2401 IPsec tunnel 50 * Well, what can I say. They impose different en/decapsulation mechanism 51 * from each other, so they need separate protocol handler. The only one 52 * we can easily determine by protocol # is IPsec, which always has 53 * AH/ESP/IPComp header right after outer IP header. 54 * 55 * So, clearly good old protosw does not work for protocol #4 and #41. 56 * The code will let you match protocol via src/dst address pair. 57 */ 58 /* XXX is M_NETADDR correct? */ 59 60 /* 61 * With USE_RADIX the code will use radix table for tunnel lookup, for 62 * tunnels registered with encap_attach() with a addr/mask pair. 63 * Faster on machines with thousands of tunnel registerations (= interfaces). 64 * 65 * The code assumes that radix table code can handle non-continuous netmask, 66 * as it will pass radix table memory region with (src + dst) sockaddr pair. 67 * 68 * FreeBSD is excluded here as they make max_keylen a static variable, and 69 * thus forbid definition of radix table other than proper domains. 70 * 71 * !!!!!!! 72 * !!NOTE: dom_maxrtkey assumes USE_RADIX is defined. 73 * !!!!!!! 74 */ 75 #define USE_RADIX 76 77 #include <sys/cdefs.h> 78 __KERNEL_RCSID(0, "$NetBSD: ip_encap.c,v 1.44 2015/04/16 06:50:16 ozaki-r Exp $"); 79 80 #include "opt_mrouting.h" 81 #include "opt_inet.h" 82 83 #include <sys/param.h> 84 #include <sys/systm.h> 85 #include <sys/socket.h> 86 #include <sys/sockio.h> 87 #include <sys/mbuf.h> 88 #include <sys/errno.h> 89 #include <sys/protosw.h> 90 #include <sys/queue.h> 91 92 #include <net/if.h> 93 #include <net/route.h> 94 95 #include <netinet/in.h> 96 #include <netinet/in_systm.h> 97 #include <netinet/ip.h> 98 #include <netinet/ip_var.h> 99 #include <netinet/ip_encap.h> 100 #ifdef MROUTING 101 #include <netinet/ip_mroute.h> 102 #endif /* MROUTING */ 103 104 #ifdef INET6 105 #include <netinet/ip6.h> 106 #include <netinet6/ip6_var.h> 107 #include <netinet6/ip6protosw.h> 108 #include <netinet6/in6_var.h> 109 #include <netinet6/in6_pcb.h> 110 #include <netinet/icmp6.h> 111 #endif 112 113 #include <net/net_osdep.h> 114 115 enum direction { INBOUND, OUTBOUND }; 116 117 #ifdef INET 118 static struct encaptab *encap4_lookup(struct mbuf *, int, int, enum direction); 119 #endif 120 #ifdef INET6 121 static struct encaptab *encap6_lookup(struct mbuf *, int, int, enum direction); 122 #endif 123 static int encap_add(struct encaptab *); 124 static int encap_remove(struct encaptab *); 125 static int encap_afcheck(int, const struct sockaddr *, const struct sockaddr *); 126 #ifdef USE_RADIX 127 static struct radix_node_head *encap_rnh(int); 128 static int mask_matchlen(const struct sockaddr *); 129 #endif 130 #ifndef USE_RADIX 131 static int mask_match(const struct encaptab *, const struct sockaddr *, 132 const struct sockaddr *); 133 #endif 134 static void encap_fillarg(struct mbuf *, const struct encaptab *); 135 136 LIST_HEAD(, encaptab) encaptab = LIST_HEAD_INITIALIZER(&encaptab); 137 138 #ifdef USE_RADIX 139 extern int max_keylen; /* radix.c */ 140 struct radix_node_head *encap_head[2]; /* 0 for AF_INET, 1 for AF_INET6 */ 141 #endif 142 143 void 144 encap_init(void) 145 { 146 static int initialized = 0; 147 148 if (initialized) 149 return; 150 initialized++; 151 #if 0 152 /* 153 * we cannot use LIST_INIT() here, since drivers may want to call 154 * encap_attach(), on driver attach. encap_init() will be called 155 * on AF_INET{,6} initialization, which happens after driver 156 * initialization - using LIST_INIT() here can nuke encap_attach() 157 * from drivers. 158 */ 159 LIST_INIT(&encaptab); 160 #endif 161 162 #ifdef USE_RADIX 163 /* 164 * initialize radix lookup table when the radix subsystem is inited. 165 */ 166 rn_delayedinit((void *)&encap_head[0], 167 sizeof(struct sockaddr_pack) << 3); 168 #ifdef INET6 169 rn_delayedinit((void *)&encap_head[1], 170 sizeof(struct sockaddr_pack) << 3); 171 #endif 172 #endif 173 } 174 175 #ifdef INET 176 static struct encaptab * 177 encap4_lookup(struct mbuf *m, int off, int proto, enum direction dir) 178 { 179 struct ip *ip; 180 struct ip_pack4 pack; 181 struct encaptab *ep, *match; 182 int prio, matchprio; 183 #ifdef USE_RADIX 184 struct radix_node_head *rnh = encap_rnh(AF_INET); 185 struct radix_node *rn; 186 #endif 187 188 KASSERT(m->m_len >= sizeof(*ip)); 189 190 ip = mtod(m, struct ip *); 191 192 memset(&pack, 0, sizeof(pack)); 193 pack.p.sp_len = sizeof(pack); 194 pack.mine.sin_family = pack.yours.sin_family = AF_INET; 195 pack.mine.sin_len = pack.yours.sin_len = sizeof(struct sockaddr_in); 196 if (dir == INBOUND) { 197 pack.mine.sin_addr = ip->ip_dst; 198 pack.yours.sin_addr = ip->ip_src; 199 } else { 200 pack.mine.sin_addr = ip->ip_src; 201 pack.yours.sin_addr = ip->ip_dst; 202 } 203 204 match = NULL; 205 matchprio = 0; 206 207 #ifdef USE_RADIX 208 rn = rnh->rnh_matchaddr((void *)&pack, rnh); 209 if (rn && (rn->rn_flags & RNF_ROOT) == 0) { 210 match = (struct encaptab *)rn; 211 matchprio = mask_matchlen(match->srcmask) + 212 mask_matchlen(match->dstmask); 213 } 214 #endif 215 216 LIST_FOREACH(ep, &encaptab, chain) { 217 if (ep->af != AF_INET) 218 continue; 219 if (ep->proto >= 0 && ep->proto != proto) 220 continue; 221 if (ep->func) 222 prio = (*ep->func)(m, off, proto, ep->arg); 223 else { 224 #ifdef USE_RADIX 225 continue; 226 #else 227 prio = mask_match(ep, (struct sockaddr *)&pack.mine, 228 (struct sockaddr *)&pack.yours); 229 #endif 230 } 231 232 /* 233 * We prioritize the matches by using bit length of the 234 * matches. mask_match() and user-supplied matching function 235 * should return the bit length of the matches (for example, 236 * if both src/dst are matched for IPv4, 64 should be returned). 237 * 0 or negative return value means "it did not match". 238 * 239 * The question is, since we have two "mask" portion, we 240 * cannot really define total order between entries. 241 * For example, which of these should be preferred? 242 * mask_match() returns 48 (32 + 16) for both of them. 243 * src=3ffe::/16, dst=3ffe:501::/32 244 * src=3ffe:501::/32, dst=3ffe::/16 245 * 246 * We need to loop through all the possible candidates 247 * to get the best match - the search takes O(n) for 248 * n attachments (i.e. interfaces). 249 * 250 * For radix-based lookup, I guess source takes precedence. 251 * See rn_{refines,lexobetter} for the correct answer. 252 */ 253 if (prio <= 0) 254 continue; 255 if (prio > matchprio) { 256 matchprio = prio; 257 match = ep; 258 } 259 } 260 261 return match; 262 } 263 264 void 265 encap4_input(struct mbuf *m, ...) 266 { 267 int off, proto; 268 va_list ap; 269 const struct protosw *psw; 270 struct encaptab *match; 271 272 va_start(ap, m); 273 off = va_arg(ap, int); 274 proto = va_arg(ap, int); 275 va_end(ap); 276 277 match = encap4_lookup(m, off, proto, INBOUND); 278 279 if (match) { 280 /* found a match, "match" has the best one */ 281 psw = match->psw; 282 if (psw && psw->pr_input) { 283 encap_fillarg(m, match); 284 (*psw->pr_input)(m, off, proto); 285 } else 286 m_freem(m); 287 return; 288 } 289 290 /* last resort: inject to raw socket */ 291 rip_input(m, off, proto); 292 } 293 #endif 294 295 #ifdef INET6 296 static struct encaptab * 297 encap6_lookup(struct mbuf *m, int off, int proto, enum direction dir) 298 { 299 struct ip6_hdr *ip6; 300 struct ip_pack6 pack; 301 int prio, matchprio; 302 struct encaptab *ep, *match; 303 #ifdef USE_RADIX 304 struct radix_node_head *rnh = encap_rnh(AF_INET6); 305 struct radix_node *rn; 306 #endif 307 308 KASSERT(m->m_len >= sizeof(*ip6)); 309 310 ip6 = mtod(m, struct ip6_hdr *); 311 312 memset(&pack, 0, sizeof(pack)); 313 pack.p.sp_len = sizeof(pack); 314 pack.mine.sin6_family = pack.yours.sin6_family = AF_INET6; 315 pack.mine.sin6_len = pack.yours.sin6_len = sizeof(struct sockaddr_in6); 316 if (dir == INBOUND) { 317 pack.mine.sin6_addr = ip6->ip6_dst; 318 pack.yours.sin6_addr = ip6->ip6_src; 319 } else { 320 pack.mine.sin6_addr = ip6->ip6_src; 321 pack.yours.sin6_addr = ip6->ip6_dst; 322 } 323 324 match = NULL; 325 matchprio = 0; 326 327 #ifdef USE_RADIX 328 rn = rnh->rnh_matchaddr((void *)&pack, rnh); 329 if (rn && (rn->rn_flags & RNF_ROOT) == 0) { 330 match = (struct encaptab *)rn; 331 matchprio = mask_matchlen(match->srcmask) + 332 mask_matchlen(match->dstmask); 333 } 334 #endif 335 336 LIST_FOREACH(ep, &encaptab, chain) { 337 if (ep->af != AF_INET6) 338 continue; 339 if (ep->proto >= 0 && ep->proto != proto) 340 continue; 341 if (ep->func) 342 prio = (*ep->func)(m, off, proto, ep->arg); 343 else { 344 #ifdef USE_RADIX 345 continue; 346 #else 347 prio = mask_match(ep, (struct sockaddr *)&pack.mine, 348 (struct sockaddr *)&pack.yours); 349 #endif 350 } 351 352 /* see encap4_lookup() for issues here */ 353 if (prio <= 0) 354 continue; 355 if (prio > matchprio) { 356 matchprio = prio; 357 match = ep; 358 } 359 } 360 361 return match; 362 } 363 364 int 365 encap6_input(struct mbuf **mp, int *offp, int proto) 366 { 367 struct mbuf *m = *mp; 368 const struct ip6protosw *psw; 369 struct encaptab *match; 370 371 match = encap6_lookup(m, *offp, proto, INBOUND); 372 373 if (match) { 374 /* found a match */ 375 psw = (const struct ip6protosw *)match->psw; 376 if (psw && psw->pr_input) { 377 encap_fillarg(m, match); 378 return (*psw->pr_input)(mp, offp, proto); 379 } else { 380 m_freem(m); 381 return IPPROTO_DONE; 382 } 383 } 384 385 /* last resort: inject to raw socket */ 386 return rip6_input(mp, offp, proto); 387 } 388 #endif 389 390 static int 391 encap_add(struct encaptab *ep) 392 { 393 #ifdef USE_RADIX 394 struct radix_node_head *rnh = encap_rnh(ep->af); 395 #endif 396 int error = 0; 397 398 LIST_INSERT_HEAD(&encaptab, ep, chain); 399 #ifdef USE_RADIX 400 if (!ep->func && rnh) { 401 if (!rnh->rnh_addaddr((void *)ep->addrpack, 402 (void *)ep->maskpack, rnh, ep->nodes)) { 403 error = EEXIST; 404 goto fail; 405 } 406 } 407 #endif 408 return error; 409 410 fail: 411 LIST_REMOVE(ep, chain); 412 return error; 413 } 414 415 static int 416 encap_remove(struct encaptab *ep) 417 { 418 #ifdef USE_RADIX 419 struct radix_node_head *rnh = encap_rnh(ep->af); 420 #endif 421 int error = 0; 422 423 LIST_REMOVE(ep, chain); 424 #ifdef USE_RADIX 425 if (!ep->func && rnh) { 426 if (!rnh->rnh_deladdr((void *)ep->addrpack, 427 (void *)ep->maskpack, rnh)) 428 error = ESRCH; 429 } 430 #endif 431 return error; 432 } 433 434 static int 435 encap_afcheck(int af, const struct sockaddr *sp, const struct sockaddr *dp) 436 { 437 if (sp && dp) { 438 if (sp->sa_len != dp->sa_len) 439 return EINVAL; 440 if (af != sp->sa_family || af != dp->sa_family) 441 return EINVAL; 442 } else if (!sp && !dp) 443 ; 444 else 445 return EINVAL; 446 447 switch (af) { 448 case AF_INET: 449 if (sp && sp->sa_len != sizeof(struct sockaddr_in)) 450 return EINVAL; 451 if (dp && dp->sa_len != sizeof(struct sockaddr_in)) 452 return EINVAL; 453 break; 454 #ifdef INET6 455 case AF_INET6: 456 if (sp && sp->sa_len != sizeof(struct sockaddr_in6)) 457 return EINVAL; 458 if (dp && dp->sa_len != sizeof(struct sockaddr_in6)) 459 return EINVAL; 460 break; 461 #endif 462 default: 463 return EAFNOSUPPORT; 464 } 465 466 return 0; 467 } 468 469 /* 470 * sp (src ptr) is always my side, and dp (dst ptr) is always remote side. 471 * length of mask (sm and dm) is assumed to be same as sp/dp. 472 * Return value will be necessary as input (cookie) for encap_detach(). 473 */ 474 const struct encaptab * 475 encap_attach(int af, int proto, 476 const struct sockaddr *sp, const struct sockaddr *sm, 477 const struct sockaddr *dp, const struct sockaddr *dm, 478 const struct protosw *psw, void *arg) 479 { 480 struct encaptab *ep; 481 int error; 482 int s; 483 size_t l; 484 struct ip_pack4 *pack4; 485 #ifdef INET6 486 struct ip_pack6 *pack6; 487 #endif 488 489 s = splsoftnet(); 490 /* sanity check on args */ 491 error = encap_afcheck(af, sp, dp); 492 if (error) 493 goto fail; 494 495 /* check if anyone have already attached with exactly same config */ 496 LIST_FOREACH(ep, &encaptab, chain) { 497 if (ep->af != af) 498 continue; 499 if (ep->proto != proto) 500 continue; 501 if (ep->func) 502 continue; 503 504 KASSERT(ep->src != NULL); 505 KASSERT(ep->dst != NULL); 506 KASSERT(ep->srcmask != NULL); 507 KASSERT(ep->dstmask != NULL); 508 509 if (ep->src->sa_len != sp->sa_len || 510 memcmp(ep->src, sp, sp->sa_len) != 0 || 511 memcmp(ep->srcmask, sm, sp->sa_len) != 0) 512 continue; 513 if (ep->dst->sa_len != dp->sa_len || 514 memcmp(ep->dst, dp, dp->sa_len) != 0 || 515 memcmp(ep->dstmask, dm, dp->sa_len) != 0) 516 continue; 517 518 error = EEXIST; 519 goto fail; 520 } 521 522 switch (af) { 523 case AF_INET: 524 l = sizeof(*pack4); 525 break; 526 #ifdef INET6 527 case AF_INET6: 528 l = sizeof(*pack6); 529 break; 530 #endif 531 default: 532 goto fail; 533 } 534 535 /* M_NETADDR ok? */ 536 ep = malloc(sizeof(*ep), M_NETADDR, M_NOWAIT|M_ZERO); 537 if (ep == NULL) { 538 error = ENOBUFS; 539 goto fail; 540 } 541 ep->addrpack = malloc(l, M_NETADDR, M_NOWAIT|M_ZERO); 542 if (ep->addrpack == NULL) { 543 error = ENOBUFS; 544 goto gc; 545 } 546 ep->maskpack = malloc(l, M_NETADDR, M_NOWAIT|M_ZERO); 547 if (ep->maskpack == NULL) { 548 error = ENOBUFS; 549 goto gc; 550 } 551 552 ep->af = af; 553 ep->proto = proto; 554 ep->addrpack->sa_len = l & 0xff; 555 ep->maskpack->sa_len = l & 0xff; 556 switch (af) { 557 case AF_INET: 558 pack4 = (struct ip_pack4 *)ep->addrpack; 559 ep->src = (struct sockaddr *)&pack4->mine; 560 ep->dst = (struct sockaddr *)&pack4->yours; 561 pack4 = (struct ip_pack4 *)ep->maskpack; 562 ep->srcmask = (struct sockaddr *)&pack4->mine; 563 ep->dstmask = (struct sockaddr *)&pack4->yours; 564 break; 565 #ifdef INET6 566 case AF_INET6: 567 pack6 = (struct ip_pack6 *)ep->addrpack; 568 ep->src = (struct sockaddr *)&pack6->mine; 569 ep->dst = (struct sockaddr *)&pack6->yours; 570 pack6 = (struct ip_pack6 *)ep->maskpack; 571 ep->srcmask = (struct sockaddr *)&pack6->mine; 572 ep->dstmask = (struct sockaddr *)&pack6->yours; 573 break; 574 #endif 575 } 576 577 memcpy(ep->src, sp, sp->sa_len); 578 memcpy(ep->srcmask, sm, sp->sa_len); 579 memcpy(ep->dst, dp, dp->sa_len); 580 memcpy(ep->dstmask, dm, dp->sa_len); 581 ep->psw = psw; 582 ep->arg = arg; 583 584 error = encap_add(ep); 585 if (error) 586 goto gc; 587 588 error = 0; 589 splx(s); 590 return ep; 591 592 gc: 593 if (ep->addrpack) 594 free(ep->addrpack, M_NETADDR); 595 if (ep->maskpack) 596 free(ep->maskpack, M_NETADDR); 597 if (ep) 598 free(ep, M_NETADDR); 599 fail: 600 splx(s); 601 return NULL; 602 } 603 604 const struct encaptab * 605 encap_attach_func(int af, int proto, 606 int (*func)(struct mbuf *, int, int, void *), 607 const struct protosw *psw, void *arg) 608 { 609 struct encaptab *ep; 610 int error; 611 int s; 612 613 s = splsoftnet(); 614 /* sanity check on args */ 615 if (!func) { 616 error = EINVAL; 617 goto fail; 618 } 619 620 error = encap_afcheck(af, NULL, NULL); 621 if (error) 622 goto fail; 623 624 ep = malloc(sizeof(*ep), M_NETADDR, M_NOWAIT); /*XXX*/ 625 if (ep == NULL) { 626 error = ENOBUFS; 627 goto fail; 628 } 629 memset(ep, 0, sizeof(*ep)); 630 631 ep->af = af; 632 ep->proto = proto; 633 ep->func = func; 634 ep->psw = psw; 635 ep->arg = arg; 636 637 error = encap_add(ep); 638 if (error) 639 goto fail; 640 641 error = 0; 642 splx(s); 643 return ep; 644 645 fail: 646 splx(s); 647 return NULL; 648 } 649 650 /* XXX encap4_ctlinput() is necessary if we set DF=1 on outer IPv4 header */ 651 652 #ifdef INET6 653 void * 654 encap6_ctlinput(int cmd, const struct sockaddr *sa, void *d0) 655 { 656 void *d = d0; 657 struct ip6_hdr *ip6; 658 struct mbuf *m; 659 int off; 660 struct ip6ctlparam *ip6cp = NULL; 661 int nxt; 662 struct encaptab *ep; 663 const struct ip6protosw *psw; 664 665 if (sa->sa_family != AF_INET6 || 666 sa->sa_len != sizeof(struct sockaddr_in6)) 667 return NULL; 668 669 if ((unsigned)cmd >= PRC_NCMDS) 670 return NULL; 671 if (cmd == PRC_HOSTDEAD) 672 d = NULL; 673 else if (cmd == PRC_MSGSIZE) 674 ; /* special code is present, see below */ 675 else if (inet6ctlerrmap[cmd] == 0) 676 return NULL; 677 678 /* if the parameter is from icmp6, decode it. */ 679 if (d != NULL) { 680 ip6cp = (struct ip6ctlparam *)d; 681 m = ip6cp->ip6c_m; 682 ip6 = ip6cp->ip6c_ip6; 683 off = ip6cp->ip6c_off; 684 nxt = ip6cp->ip6c_nxt; 685 686 if (ip6 && cmd == PRC_MSGSIZE) { 687 int valid = 0; 688 struct encaptab *match; 689 690 /* 691 * Check to see if we have a valid encap configuration. 692 */ 693 match = encap6_lookup(m, off, nxt, OUTBOUND); 694 if (match) 695 valid++; 696 697 /* 698 * Depending on the value of "valid" and routing table 699 * size (mtudisc_{hi,lo}wat), we will: 700 * - recalcurate the new MTU and create the 701 * corresponding routing entry, or 702 * - ignore the MTU change notification. 703 */ 704 icmp6_mtudisc_update((struct ip6ctlparam *)d, valid); 705 } 706 } else { 707 m = NULL; 708 ip6 = NULL; 709 nxt = -1; 710 } 711 712 /* inform all listeners */ 713 LIST_FOREACH(ep, &encaptab, chain) { 714 if (ep->af != AF_INET6) 715 continue; 716 if (ep->proto >= 0 && ep->proto != nxt) 717 continue; 718 719 /* should optimize by looking at address pairs */ 720 721 /* XXX need to pass ep->arg or ep itself to listeners */ 722 psw = (const struct ip6protosw *)ep->psw; 723 if (psw && psw->pr_ctlinput) 724 (*psw->pr_ctlinput)(cmd, sa, d); 725 } 726 727 rip6_ctlinput(cmd, sa, d0); 728 return NULL; 729 } 730 #endif 731 732 int 733 encap_detach(const struct encaptab *cookie) 734 { 735 const struct encaptab *ep = cookie; 736 struct encaptab *p, *np; 737 int error; 738 739 LIST_FOREACH_SAFE(p, &encaptab, chain, np) { 740 if (p == ep) { 741 error = encap_remove(p); 742 if (error) 743 return error; 744 if (!ep->func) { 745 free(p->addrpack, M_NETADDR); 746 free(p->maskpack, M_NETADDR); 747 } 748 free(p, M_NETADDR); /*XXX*/ 749 return 0; 750 } 751 } 752 753 return ENOENT; 754 } 755 756 #ifdef USE_RADIX 757 static struct radix_node_head * 758 encap_rnh(int af) 759 { 760 761 switch (af) { 762 case AF_INET: 763 return encap_head[0]; 764 #ifdef INET6 765 case AF_INET6: 766 return encap_head[1]; 767 #endif 768 default: 769 return NULL; 770 } 771 } 772 773 static int 774 mask_matchlen(const struct sockaddr *sa) 775 { 776 const char *p, *ep; 777 int l; 778 779 p = (const char *)sa; 780 ep = p + sa->sa_len; 781 p += 2; /* sa_len + sa_family */ 782 783 l = 0; 784 while (p < ep) { 785 l += (*p ? 8 : 0); /* estimate */ 786 p++; 787 } 788 return l; 789 } 790 #endif 791 792 #ifndef USE_RADIX 793 static int 794 mask_match(const struct encaptab *ep, 795 const struct sockaddr *sp, 796 const struct sockaddr *dp) 797 { 798 struct sockaddr_storage s; 799 struct sockaddr_storage d; 800 int i; 801 const u_int8_t *p, *q; 802 u_int8_t *r; 803 int matchlen; 804 805 KASSERTMSG(ep->func == NULL, "wrong encaptab passed to mask_match"); 806 807 if (sp->sa_len > sizeof(s) || dp->sa_len > sizeof(d)) 808 return 0; 809 if (sp->sa_family != ep->af || dp->sa_family != ep->af) 810 return 0; 811 if (sp->sa_len != ep->src->sa_len || dp->sa_len != ep->dst->sa_len) 812 return 0; 813 814 matchlen = 0; 815 816 p = (const u_int8_t *)sp; 817 q = (const u_int8_t *)ep->srcmask; 818 r = (u_int8_t *)&s; 819 for (i = 0 ; i < sp->sa_len; i++) { 820 r[i] = p[i] & q[i]; 821 /* XXX estimate */ 822 matchlen += (q[i] ? 8 : 0); 823 } 824 825 p = (const u_int8_t *)dp; 826 q = (const u_int8_t *)ep->dstmask; 827 r = (u_int8_t *)&d; 828 for (i = 0 ; i < dp->sa_len; i++) { 829 r[i] = p[i] & q[i]; 830 /* XXX rough estimate */ 831 matchlen += (q[i] ? 8 : 0); 832 } 833 834 /* need to overwrite len/family portion as we don't compare them */ 835 s.ss_len = sp->sa_len; 836 s.ss_family = sp->sa_family; 837 d.ss_len = dp->sa_len; 838 d.ss_family = dp->sa_family; 839 840 if (memcmp(&s, ep->src, ep->src->sa_len) == 0 && 841 memcmp(&d, ep->dst, ep->dst->sa_len) == 0) { 842 return matchlen; 843 } else 844 return 0; 845 } 846 #endif 847 848 static void 849 encap_fillarg(struct mbuf *m, const struct encaptab *ep) 850 { 851 struct m_tag *mtag; 852 853 mtag = m_tag_get(PACKET_TAG_ENCAP, sizeof(void *), M_NOWAIT); 854 if (mtag) { 855 *(void **)(mtag + 1) = ep->arg; 856 m_tag_prepend(m, mtag); 857 } 858 } 859 860 void * 861 encap_getarg(struct mbuf *m) 862 { 863 void *p; 864 struct m_tag *mtag; 865 866 p = NULL; 867 mtag = m_tag_find(m, PACKET_TAG_ENCAP, NULL); 868 if (mtag != NULL) { 869 p = *(void **)(mtag + 1); 870 m_tag_delete(m, mtag); 871 } 872 return p; 873 } 874