1 /* $NetBSD: altq_subr.c,v 1.34 2025/01/08 13:00:04 joe Exp $ */ 2 /* $KAME: altq_subr.c,v 1.24 2005/04/13 03:44:25 suz Exp $ */ 3 4 /* 5 * Copyright (C) 1997-2003 6 * Sony Computer Science Laboratories Inc. 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 * 17 * THIS SOFTWARE IS PROVIDED BY SONY CSL AND CONTRIBUTORS ``AS IS'' AND 18 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 * ARE DISCLAIMED. IN NO EVENT SHALL SONY CSL OR CONTRIBUTORS BE LIABLE 21 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 23 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 24 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 25 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 26 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 27 * SUCH DAMAGE. 28 */ 29 30 #include <sys/cdefs.h> 31 __KERNEL_RCSID(0, "$NetBSD: altq_subr.c,v 1.34 2025/01/08 13:00:04 joe Exp $"); 32 33 #ifdef _KERNEL_OPT 34 #include "opt_altq.h" 35 #include "opt_inet.h" 36 #include "pf.h" 37 #endif 38 39 #include <sys/param.h> 40 #include <sys/malloc.h> 41 #include <sys/mbuf.h> 42 #include <sys/systm.h> 43 #include <sys/proc.h> 44 #include <sys/socket.h> 45 #include <sys/socketvar.h> 46 #include <sys/kernel.h> 47 #include <sys/errno.h> 48 #include <sys/syslog.h> 49 #include <sys/sysctl.h> 50 #include <sys/queue.h> 51 52 #include <net/if.h> 53 #include <net/if_dl.h> 54 #include <net/if_types.h> 55 56 #include <netinet/in.h> 57 #include <netinet/in_systm.h> 58 #include <netinet/ip.h> 59 #ifdef INET6 60 #include <netinet/ip6.h> 61 #endif 62 #include <netinet/tcp.h> 63 #include <netinet/udp.h> 64 65 #if NPF > 0 66 #include <net/pfvar.h> 67 #endif 68 #include <altq/altq.h> 69 #ifdef ALTQ3_COMPAT 70 #include <altq/altq_conf.h> 71 #endif 72 73 /* 74 * internal function prototypes 75 */ 76 static void tbr_timeout(void *); 77 int (*altq_input)(struct mbuf *, int) = NULL; 78 static int tbr_timer = 0; /* token bucket regulator timer */ 79 static struct callout tbr_callout; 80 81 #ifdef ALTQ3_CLFIER_COMPAT 82 static int extract_ports4(struct mbuf *, struct ip *, struct flowinfo_in *); 83 #ifdef INET6 84 static int extract_ports6(struct mbuf *, struct ip6_hdr *, 85 struct flowinfo_in6 *); 86 #endif 87 static int apply_filter4(u_int32_t, struct flow_filter *, 88 struct flowinfo_in *); 89 static int apply_ppfilter4(u_int32_t, struct flow_filter *, 90 struct flowinfo_in *); 91 #ifdef INET6 92 static int apply_filter6(u_int32_t, struct flow_filter6 *, 93 struct flowinfo_in6 *); 94 #endif 95 static int apply_tosfilter4(u_int32_t, struct flow_filter *, 96 struct flowinfo_in *); 97 static u_long get_filt_handle(struct acc_classifier *, int); 98 static struct acc_filter *filth_to_filtp(struct acc_classifier *, u_long); 99 static u_int32_t filt2fibmask(struct flow_filter *); 100 101 static void ip4f_cache(struct ip *, struct flowinfo_in *); 102 static int ip4f_lookup(struct ip *, struct flowinfo_in *); 103 static int ip4f_init(void); 104 static struct ip4_frag *ip4f_alloc(void); 105 static void ip4f_free(struct ip4_frag *); 106 #endif /* ALTQ3_CLFIER_COMPAT */ 107 108 /* 109 * alternate queueing support routines 110 */ 111 112 /* look up the queue state by the interface name and the queueing type. */ 113 void * 114 altq_lookup(char *name, int type) 115 { 116 struct ifnet *ifp; 117 118 if ((ifp = ifunit(name)) != NULL) { 119 if (type != ALTQT_NONE && ifp->if_snd.altq_type == type) 120 return (ifp->if_snd.altq_disc); 121 } 122 123 return NULL; 124 } 125 126 int 127 altq_attach(struct ifaltq *ifq, int type, void *discipline, 128 int (*enqueue)(struct ifaltq *, struct mbuf *), 129 struct mbuf *(*dequeue)(struct ifaltq *, int), 130 int (*request)(struct ifaltq *, int, void *), 131 void *clfier, void *(*classify)(void *, struct mbuf *, int)) 132 { 133 if (!ALTQ_IS_READY(ifq)) 134 return ENXIO; 135 136 #ifdef ALTQ3_COMPAT 137 /* 138 * pfaltq can override the existing discipline, but altq3 cannot. 139 * check these if clfier is not NULL (which implies altq3). 140 */ 141 if (clfier != NULL) { 142 if (ALTQ_IS_ENABLED(ifq)) 143 return EBUSY; 144 if (ALTQ_IS_ATTACHED(ifq)) 145 return EEXIST; 146 } 147 #endif 148 ifq->altq_type = type; 149 ifq->altq_disc = discipline; 150 ifq->altq_enqueue = enqueue; 151 ifq->altq_dequeue = dequeue; 152 ifq->altq_request = request; 153 ifq->altq_clfier = clfier; 154 ifq->altq_classify = classify; 155 ifq->altq_flags &= (ALTQF_CANTCHANGE|ALTQF_ENABLED); 156 #ifdef ALTQ3_COMPAT 157 #ifdef ALTQ_KLD 158 altq_module_incref(type); 159 #endif 160 #endif 161 return 0; 162 } 163 164 int 165 altq_detach(struct ifaltq *ifq) 166 { 167 if (!ALTQ_IS_READY(ifq)) 168 return ENXIO; 169 if (ALTQ_IS_ENABLED(ifq)) 170 return EBUSY; 171 if (!ALTQ_IS_ATTACHED(ifq)) 172 return 0; 173 #ifdef ALTQ3_COMPAT 174 #ifdef ALTQ_KLD 175 altq_module_declref(ifq->altq_type); 176 #endif 177 #endif 178 179 ifq->altq_type = ALTQT_NONE; 180 ifq->altq_disc = NULL; 181 ifq->altq_enqueue = NULL; 182 ifq->altq_dequeue = NULL; 183 ifq->altq_request = NULL; 184 ifq->altq_clfier = NULL; 185 ifq->altq_classify = NULL; 186 ifq->altq_flags &= ALTQF_CANTCHANGE; 187 return 0; 188 } 189 190 int 191 altq_enable(struct ifaltq *ifq) 192 { 193 int s; 194 195 if (!ALTQ_IS_READY(ifq)) 196 return ENXIO; 197 if (ALTQ_IS_ENABLED(ifq)) 198 return 0; 199 200 s = splnet(); 201 IFQ_PURGE(ifq); 202 ASSERT(ifq->ifq_len == 0); 203 ifq->altq_flags |= ALTQF_ENABLED; 204 if (ifq->altq_clfier != NULL) 205 ifq->altq_flags |= ALTQF_CLASSIFY; 206 splx(s); 207 208 return 0; 209 } 210 211 int 212 altq_disable(struct ifaltq *ifq) 213 { 214 int s; 215 216 if (!ALTQ_IS_ENABLED(ifq)) 217 return 0; 218 219 s = splnet(); 220 IFQ_PURGE(ifq); 221 ASSERT(ifq->ifq_len == 0); 222 ifq->altq_flags &= ~(ALTQF_ENABLED|ALTQF_CLASSIFY); 223 splx(s); 224 return 0; 225 } 226 227 #ifdef ALTQ_DEBUG 228 void 229 altq_assert(const char *file, int line, const char *failedexpr) 230 { 231 (void)printf("altq assertion \"%s\" failed: file \"%s\", line %d\n", 232 failedexpr, file, line); 233 panic("altq assertion"); 234 /* NOTREACHED */ 235 } 236 #endif 237 238 /* 239 * internal representation of token bucket parameters 240 * rate: byte_per_unittime << 32 241 * (((bits_per_sec) / 8) << 32) / machclk_freq 242 * depth: byte << 32 243 * 244 */ 245 #define TBR_SHIFT 32 246 #define TBR_SCALE(x) ((int64_t)(x) << TBR_SHIFT) 247 #define TBR_UNSCALE(x) ((x) >> TBR_SHIFT) 248 249 struct mbuf * 250 tbr_dequeue(struct ifaltq *ifq, int op) 251 { 252 struct tb_regulator *tbr; 253 struct mbuf *m; 254 int64_t interval; 255 u_int64_t now; 256 257 tbr = ifq->altq_tbr; 258 if (op == ALTDQ_REMOVE && tbr->tbr_lastop == ALTDQ_POLL) { 259 /* if this is a remove after poll, bypass tbr check */ 260 } else { 261 /* update token only when it is negative */ 262 if (tbr->tbr_token <= 0) { 263 now = read_machclk(); 264 interval = now - tbr->tbr_last; 265 if (interval >= tbr->tbr_filluptime) 266 tbr->tbr_token = tbr->tbr_depth; 267 else { 268 tbr->tbr_token += interval * tbr->tbr_rate; 269 if (tbr->tbr_token > tbr->tbr_depth) 270 tbr->tbr_token = tbr->tbr_depth; 271 } 272 tbr->tbr_last = now; 273 } 274 /* if token is still negative, don't allow dequeue */ 275 if (tbr->tbr_token <= 0) 276 return NULL; 277 } 278 279 if (ALTQ_IS_ENABLED(ifq)) 280 m = (*ifq->altq_dequeue)(ifq, op); 281 else { 282 if (op == ALTDQ_POLL) 283 IF_POLL(ifq, m); 284 else 285 IF_DEQUEUE(ifq, m); 286 } 287 288 if (m != NULL && op == ALTDQ_REMOVE) 289 tbr->tbr_token -= TBR_SCALE(m_pktlen(m)); 290 tbr->tbr_lastop = op; 291 return (m); 292 } 293 294 /* 295 * set a token bucket regulator. 296 * if the specified rate is zero, the token bucket regulator is deleted. 297 */ 298 int 299 tbr_set(struct ifaltq *ifq, struct tb_profile *profile) 300 { 301 struct tb_regulator *tbr, *otbr; 302 303 if (machclk_freq == 0) 304 init_machclk(); 305 if (machclk_freq == 0) { 306 printf("tbr_set: no CPU clock available!\n"); 307 return ENXIO; 308 } 309 310 if (profile->rate == 0) { 311 /* delete this tbr */ 312 if ((tbr = ifq->altq_tbr) == NULL) 313 return ENOENT; 314 ifq->altq_tbr = NULL; 315 free(tbr, M_DEVBUF); 316 return 0; 317 } 318 319 tbr = malloc(sizeof(struct tb_regulator), M_DEVBUF, M_WAITOK|M_ZERO); 320 if (tbr == NULL) 321 return ENOMEM; 322 323 tbr->tbr_rate = TBR_SCALE(profile->rate / 8) / machclk_freq; 324 tbr->tbr_depth = TBR_SCALE(profile->depth); 325 if (tbr->tbr_rate > 0) 326 tbr->tbr_filluptime = tbr->tbr_depth / tbr->tbr_rate; 327 else 328 tbr->tbr_filluptime = 0xffffffffffffffffLL; 329 tbr->tbr_token = tbr->tbr_depth; 330 tbr->tbr_last = read_machclk(); 331 tbr->tbr_lastop = ALTDQ_REMOVE; 332 333 otbr = ifq->altq_tbr; 334 ifq->altq_tbr = tbr; /* set the new tbr */ 335 336 if (otbr != NULL) { 337 free(otbr, M_DEVBUF); 338 } else { 339 if (tbr_timer == 0) { 340 CALLOUT_RESET(&tbr_callout, 1, tbr_timeout, (void *)0); 341 tbr_timer = 1; 342 } 343 } 344 return 0; 345 } 346 347 /* 348 * tbr_timeout goes through the interface list, and kicks the drivers 349 * if necessary. 350 */ 351 static void 352 tbr_timeout(void *arg) 353 { 354 struct ifnet *ifp; 355 int active, s; 356 357 active = 0; 358 s = pserialize_read_enter(); 359 IFNET_READER_FOREACH(ifp) { 360 struct psref psref; 361 if (!TBR_IS_ENABLED(&ifp->if_snd)) 362 continue; 363 if_acquire(ifp, &psref); 364 pserialize_read_exit(s); 365 366 active++; 367 if (!IFQ_IS_EMPTY(&ifp->if_snd) && ifp->if_start != NULL) { 368 int _s = splnet(); 369 if_start_lock(ifp); 370 splx(_s); 371 } 372 373 s = pserialize_read_enter(); 374 if_release(ifp, &psref); 375 } 376 pserialize_read_exit(s); 377 378 if (active > 0) 379 CALLOUT_RESET(&tbr_callout, 1, tbr_timeout, (void *)0); 380 else 381 tbr_timer = 0; /* don't need tbr_timer anymore */ 382 } 383 384 /* 385 * get token bucket regulator profile 386 */ 387 int 388 tbr_get(struct ifaltq *ifq, struct tb_profile *profile) 389 { 390 struct tb_regulator *tbr; 391 392 if ((tbr = ifq->altq_tbr) == NULL) { 393 profile->rate = 0; 394 profile->depth = 0; 395 } else { 396 profile->rate = 397 (u_int)TBR_UNSCALE(tbr->tbr_rate * 8 * machclk_freq); 398 profile->depth = (u_int)TBR_UNSCALE(tbr->tbr_depth); 399 } 400 return 0; 401 } 402 403 #if NPF > 0 404 /* 405 * attach a discipline to the interface. if one already exists, it is 406 * overridden. 407 */ 408 int 409 altq_pfattach(struct pf_altq *a) 410 { 411 int error = 0; 412 413 switch (a->scheduler) { 414 case ALTQT_NONE: 415 break; 416 #ifdef ALTQ_CBQ 417 case ALTQT_CBQ: 418 error = cbq_pfattach(a); 419 break; 420 #endif 421 #ifdef ALTQ_PRIQ 422 case ALTQT_PRIQ: 423 error = priq_pfattach(a); 424 break; 425 #endif 426 #ifdef ALTQ_HFSC 427 case ALTQT_HFSC: 428 error = hfsc_pfattach(a); 429 break; 430 #endif 431 default: 432 error = ENXIO; 433 } 434 435 return error; 436 } 437 438 /* 439 * detach a discipline from the interface. 440 * it is possible that the discipline was already overridden by another 441 * discipline. 442 */ 443 int 444 altq_pfdetach(struct pf_altq *a) 445 { 446 struct ifnet *ifp; 447 int s, error = 0; 448 449 if ((ifp = ifunit(a->ifname)) == NULL) 450 return EINVAL; 451 452 /* if this discipline is no longer referenced, just return */ 453 if (a->altq_disc == NULL || a->altq_disc != ifp->if_snd.altq_disc) 454 return 0; 455 456 s = splnet(); 457 if (ALTQ_IS_ENABLED(&ifp->if_snd)) 458 error = altq_disable(&ifp->if_snd); 459 if (error == 0) 460 error = altq_detach(&ifp->if_snd); 461 splx(s); 462 463 return error; 464 } 465 466 /* 467 * add a discipline or a queue 468 */ 469 int 470 altq_add(struct pf_altq *a) 471 { 472 int error = 0; 473 474 if (a->qname[0] != 0) 475 return (altq_add_queue(a)); 476 477 if (machclk_freq == 0) 478 init_machclk(); 479 if (machclk_freq == 0) 480 panic("altq_add: no CPU clock"); 481 482 switch (a->scheduler) { 483 #ifdef ALTQ_CBQ 484 case ALTQT_CBQ: 485 error = cbq_add_altq(a); 486 break; 487 #endif 488 #ifdef ALTQ_PRIQ 489 case ALTQT_PRIQ: 490 error = priq_add_altq(a); 491 break; 492 #endif 493 #ifdef ALTQ_HFSC 494 case ALTQT_HFSC: 495 error = hfsc_add_altq(a); 496 break; 497 #endif 498 default: 499 error = ENXIO; 500 } 501 502 return error; 503 } 504 505 /* 506 * remove a discipline or a queue 507 */ 508 int 509 altq_remove(struct pf_altq *a) 510 { 511 int error = 0; 512 513 if (a->qname[0] != 0) 514 return (altq_remove_queue(a)); 515 516 switch (a->scheduler) { 517 #ifdef ALTQ_CBQ 518 case ALTQT_CBQ: 519 error = cbq_remove_altq(a); 520 break; 521 #endif 522 #ifdef ALTQ_PRIQ 523 case ALTQT_PRIQ: 524 error = priq_remove_altq(a); 525 break; 526 #endif 527 #ifdef ALTQ_HFSC 528 case ALTQT_HFSC: 529 error = hfsc_remove_altq(a); 530 break; 531 #endif 532 default: 533 error = ENXIO; 534 } 535 536 return error; 537 } 538 539 /* 540 * add a queue to the discipline 541 */ 542 int 543 altq_add_queue(struct pf_altq *a) 544 { 545 int error = 0; 546 547 switch (a->scheduler) { 548 #ifdef ALTQ_CBQ 549 case ALTQT_CBQ: 550 error = cbq_add_queue(a); 551 break; 552 #endif 553 #ifdef ALTQ_PRIQ 554 case ALTQT_PRIQ: 555 error = priq_add_queue(a); 556 break; 557 #endif 558 #ifdef ALTQ_HFSC 559 case ALTQT_HFSC: 560 error = hfsc_add_queue(a); 561 break; 562 #endif 563 default: 564 error = ENXIO; 565 } 566 567 return error; 568 } 569 570 /* 571 * remove a queue from the discipline 572 */ 573 int 574 altq_remove_queue(struct pf_altq *a) 575 { 576 int error = 0; 577 578 switch (a->scheduler) { 579 #ifdef ALTQ_CBQ 580 case ALTQT_CBQ: 581 error = cbq_remove_queue(a); 582 break; 583 #endif 584 #ifdef ALTQ_PRIQ 585 case ALTQT_PRIQ: 586 error = priq_remove_queue(a); 587 break; 588 #endif 589 #ifdef ALTQ_HFSC 590 case ALTQT_HFSC: 591 error = hfsc_remove_queue(a); 592 break; 593 #endif 594 default: 595 error = ENXIO; 596 } 597 598 return error; 599 } 600 601 /* 602 * get queue statistics 603 */ 604 int 605 altq_getqstats(struct pf_altq *a, void *ubuf, int *nbytes) 606 { 607 int error = 0; 608 609 switch (a->scheduler) { 610 #ifdef ALTQ_CBQ 611 case ALTQT_CBQ: 612 error = cbq_getqstats(a, ubuf, nbytes); 613 break; 614 #endif 615 #ifdef ALTQ_PRIQ 616 case ALTQT_PRIQ: 617 error = priq_getqstats(a, ubuf, nbytes); 618 break; 619 #endif 620 #ifdef ALTQ_HFSC 621 case ALTQT_HFSC: 622 error = hfsc_getqstats(a, ubuf, nbytes); 623 break; 624 #endif 625 default: 626 error = ENXIO; 627 } 628 629 return error; 630 } 631 #endif /* NPF > 0 */ 632 633 /* 634 * read and write diffserv field in IPv4 or IPv6 header 635 */ 636 u_int8_t 637 read_dsfield(struct mbuf *m, struct altq_pktattr *pktattr) 638 { 639 struct mbuf *m0; 640 u_int8_t ds_field = 0; 641 642 if (pktattr == NULL || 643 (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6)) 644 return ((u_int8_t)0); 645 646 /* verify that pattr_hdr is within the mbuf data */ 647 for (m0 = m; m0 != NULL; m0 = m0->m_next) 648 if (((char *)pktattr->pattr_hdr >= m0->m_data) && 649 ((char *)pktattr->pattr_hdr < m0->m_data + m0->m_len)) 650 break; 651 if (m0 == NULL) { 652 /* ick, pattr_hdr is stale */ 653 pktattr->pattr_af = AF_UNSPEC; 654 #ifdef ALTQ_DEBUG 655 printf("read_dsfield: can't locate header!\n"); 656 #endif 657 return ((u_int8_t)0); 658 } 659 660 if (pktattr->pattr_af == AF_INET) { 661 struct ip *ip = (struct ip *)pktattr->pattr_hdr; 662 663 if (ip->ip_v != 4) 664 return ((u_int8_t)0); /* version mismatch! */ 665 ds_field = ip->ip_tos; 666 } 667 #ifdef INET6 668 else if (pktattr->pattr_af == AF_INET6) { 669 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr; 670 u_int32_t flowlabel; 671 672 flowlabel = ntohl(ip6->ip6_flow); 673 if ((flowlabel >> 28) != 6) 674 return ((u_int8_t)0); /* version mismatch! */ 675 ds_field = (flowlabel >> 20) & 0xff; 676 } 677 #endif 678 return ds_field; 679 } 680 681 void 682 write_dsfield(struct mbuf *m, struct altq_pktattr *pktattr, u_int8_t dsfield) 683 { 684 struct mbuf *m0; 685 686 if (pktattr == NULL || 687 (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6)) 688 return; 689 690 /* verify that pattr_hdr is within the mbuf data */ 691 for (m0 = m; m0 != NULL; m0 = m0->m_next) 692 if (((char *)pktattr->pattr_hdr >= m0->m_data) && 693 ((char *)pktattr->pattr_hdr < m0->m_data + m0->m_len)) 694 break; 695 if (m0 == NULL) { 696 /* ick, pattr_hdr is stale */ 697 pktattr->pattr_af = AF_UNSPEC; 698 #ifdef ALTQ_DEBUG 699 printf("write_dsfield: can't locate header!\n"); 700 #endif 701 return; 702 } 703 704 if (pktattr->pattr_af == AF_INET) { 705 struct ip *ip = (struct ip *)pktattr->pattr_hdr; 706 u_int8_t old; 707 int32_t sum; 708 709 if (ip->ip_v != 4) 710 return; /* version mismatch! */ 711 old = ip->ip_tos; 712 dsfield |= old & 3; /* leave CU bits */ 713 if (old == dsfield) 714 return; 715 ip->ip_tos = dsfield; 716 /* 717 * update checksum (from RFC1624) 718 * HC' = ~(~HC + ~m + m') 719 */ 720 sum = ~ntohs(ip->ip_sum) & 0xffff; 721 sum += 0xff00 + (~old & 0xff) + dsfield; 722 sum = (sum >> 16) + (sum & 0xffff); 723 sum += (sum >> 16); /* add carry */ 724 725 ip->ip_sum = htons(~sum & 0xffff); 726 } 727 #ifdef INET6 728 else if (pktattr->pattr_af == AF_INET6) { 729 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr; 730 u_int32_t flowlabel; 731 732 flowlabel = ntohl(ip6->ip6_flow); 733 if ((flowlabel >> 28) != 6) 734 return; /* version mismatch! */ 735 flowlabel = (flowlabel & 0xf03fffff) | (dsfield << 20); 736 ip6->ip6_flow = htonl(flowlabel); 737 } 738 #endif 739 return; 740 } 741 742 #define BINTIME_SHIFT 2 743 744 u_int32_t machclk_freq = 0; 745 u_int32_t machclk_per_tick = 0; 746 747 void 748 init_machclk(void) 749 { 750 751 callout_init(&tbr_callout, 0); 752 753 /* 754 * Always emulate 1GiHz counter using bintime(9) 755 * since it has enough resolution via timecounter(9). 756 * Using machine dependent cpu_counter() is not MP safe 757 * and it won't work even on UP with Speedstep etc. 758 */ 759 machclk_freq = 1024 * 1024 * 1024; /* 2^30 to emulate ~1GHz */ 760 machclk_per_tick = machclk_freq / hz; 761 #ifdef ALTQ_DEBUG 762 printf("altq: emulate %uHz CPU clock\n", machclk_freq); 763 #endif 764 } 765 766 u_int64_t 767 read_machclk(void) 768 { 769 struct bintime bt; 770 u_int64_t val; 771 772 binuptime(&bt); 773 val = (((u_int64_t)bt.sec << 32) + (bt.frac >> 32)) >> BINTIME_SHIFT; 774 return val; 775 } 776 777 #ifdef ALTQ3_CLFIER_COMPAT 778 779 #ifndef IPPROTO_ESP 780 #define IPPROTO_ESP 50 /* encapsulating security payload */ 781 #endif 782 #ifndef IPPROTO_AH 783 #define IPPROTO_AH 51 /* authentication header */ 784 #endif 785 786 /* 787 * extract flow information from a given packet. 788 * filt_mask shows flowinfo fields required. 789 * we assume the ip header is in one mbuf, and addresses and ports are 790 * in network byte order. 791 */ 792 int 793 altq_extractflow(struct mbuf *m, int af, struct flowinfo *flow, 794 u_int32_t filt_bmask) 795 { 796 797 switch (af) { 798 case PF_INET: { 799 struct flowinfo_in *fin; 800 struct ip *ip; 801 802 ip = mtod(m, struct ip *); 803 804 if (ip->ip_v != 4) 805 break; 806 807 fin = (struct flowinfo_in *)flow; 808 fin->fi_len = sizeof(struct flowinfo_in); 809 fin->fi_family = AF_INET; 810 811 fin->fi_proto = ip->ip_p; 812 fin->fi_tos = ip->ip_tos; 813 814 fin->fi_src.s_addr = ip->ip_src.s_addr; 815 fin->fi_dst.s_addr = ip->ip_dst.s_addr; 816 817 if (filt_bmask & FIMB4_PORTS) 818 /* if port info is required, extract port numbers */ 819 extract_ports4(m, ip, fin); 820 else { 821 fin->fi_sport = 0; 822 fin->fi_dport = 0; 823 fin->fi_gpi = 0; 824 } 825 return 1; 826 } 827 828 #ifdef INET6 829 case PF_INET6: { 830 struct flowinfo_in6 *fin6; 831 struct ip6_hdr *ip6; 832 833 ip6 = mtod(m, struct ip6_hdr *); 834 /* should we check the ip version? */ 835 836 fin6 = (struct flowinfo_in6 *)flow; 837 fin6->fi6_len = sizeof(struct flowinfo_in6); 838 fin6->fi6_family = AF_INET6; 839 840 fin6->fi6_proto = ip6->ip6_nxt; 841 fin6->fi6_tclass = (ntohl(ip6->ip6_flow) >> 20) & 0xff; 842 843 fin6->fi6_flowlabel = ip6->ip6_flow & htonl(0x000fffff); 844 fin6->fi6_src = ip6->ip6_src; 845 fin6->fi6_dst = ip6->ip6_dst; 846 847 if ((filt_bmask & FIMB6_PORTS) || 848 ((filt_bmask & FIMB6_PROTO) 849 && ip6->ip6_nxt > IPPROTO_IPV6)) 850 /* 851 * if port info is required, or proto is required 852 * but there are option headers, extract port 853 * and protocol numbers. 854 */ 855 extract_ports6(m, ip6, fin6); 856 else { 857 fin6->fi6_sport = 0; 858 fin6->fi6_dport = 0; 859 fin6->fi6_gpi = 0; 860 } 861 return 1; 862 } 863 #endif /* INET6 */ 864 865 default: 866 break; 867 } 868 869 /* failed */ 870 flow->fi_len = sizeof(struct flowinfo); 871 flow->fi_family = AF_UNSPEC; 872 return 0; 873 } 874 875 /* 876 * helper routine to extract port numbers 877 */ 878 /* structure for ipsec and ipv6 option header template */ 879 struct _opt6 { 880 u_int8_t opt6_nxt; /* next header */ 881 u_int8_t opt6_hlen; /* header extension length */ 882 u_int16_t _pad; 883 u_int32_t ah_spi; /* security parameter index 884 for authentication header */ 885 }; 886 887 /* 888 * extract port numbers from a ipv4 packet. 889 */ 890 static int 891 extract_ports4(struct mbuf *m, struct ip *ip, struct flowinfo_in *fin) 892 { 893 struct mbuf *m0; 894 u_short ip_off; 895 u_int8_t proto; 896 int off; 897 898 fin->fi_sport = 0; 899 fin->fi_dport = 0; 900 fin->fi_gpi = 0; 901 902 ip_off = ntohs(ip->ip_off); 903 /* if it is a fragment, try cached fragment info */ 904 if (ip_off & IP_OFFMASK) { 905 ip4f_lookup(ip, fin); 906 return 1; 907 } 908 909 /* locate the mbuf containing the protocol header */ 910 for (m0 = m; m0 != NULL; m0 = m0->m_next) 911 if (((char *)ip >= m0->m_data) && 912 ((char *)ip < m0->m_data + m0->m_len)) 913 break; 914 if (m0 == NULL) { 915 #ifdef ALTQ_DEBUG 916 printf("extract_ports4: can't locate header! ip=%p\n", ip); 917 #endif 918 return 0; 919 } 920 off = ((char *)ip - m0->m_data) + (ip->ip_hl << 2); 921 proto = ip->ip_p; 922 923 #ifdef ALTQ_IPSEC 924 again: 925 #endif 926 while (off >= m0->m_len) { 927 off -= m0->m_len; 928 m0 = m0->m_next; 929 if (m0 == NULL) 930 return 0; /* bogus ip_hl! */ 931 } 932 if (m0->m_len < off + 4) 933 return 0; 934 935 switch (proto) { 936 case IPPROTO_TCP: 937 case IPPROTO_UDP: { 938 struct udphdr *udp; 939 940 udp = (struct udphdr *)(mtod(m0, char *) + off); 941 fin->fi_sport = udp->uh_sport; 942 fin->fi_dport = udp->uh_dport; 943 fin->fi_proto = proto; 944 } 945 break; 946 947 #ifdef ALTQ_IPSEC 948 case IPPROTO_ESP: 949 if (fin->fi_gpi == 0){ 950 u_int32_t *gpi; 951 952 gpi = (u_int32_t *)(mtod(m0, char *) + off); 953 fin->fi_gpi = *gpi; 954 } 955 fin->fi_proto = proto; 956 break; 957 958 case IPPROTO_AH: { 959 /* get next header and header length */ 960 struct _opt6 *opt6; 961 962 opt6 = (struct _opt6 *)(mtod(m0, char *) + off); 963 proto = opt6->opt6_nxt; 964 off += 8 + (opt6->opt6_hlen * 4); 965 if (fin->fi_gpi == 0 && m0->m_len >= off + 8) 966 fin->fi_gpi = opt6->ah_spi; 967 } 968 /* goto the next header */ 969 goto again; 970 #endif /* ALTQ_IPSEC */ 971 972 default: 973 fin->fi_proto = proto; 974 return 0; 975 } 976 977 /* if this is a first fragment, cache it. */ 978 if (ip_off & IP_MF) 979 ip4f_cache(ip, fin); 980 981 return 1; 982 } 983 984 #ifdef INET6 985 static int 986 extract_ports6(struct mbuf *m, struct ip6_hdr *ip6, struct flowinfo_in6 *fin6) 987 { 988 struct mbuf *m0; 989 int off; 990 u_int8_t proto; 991 992 fin6->fi6_gpi = 0; 993 fin6->fi6_sport = 0; 994 fin6->fi6_dport = 0; 995 996 /* locate the mbuf containing the protocol header */ 997 for (m0 = m; m0 != NULL; m0 = m0->m_next) 998 if (((char *)ip6 >= m0->m_data) && 999 ((char *)ip6 < m0->m_data + m0->m_len)) 1000 break; 1001 if (m0 == NULL) { 1002 #ifdef ALTQ_DEBUG 1003 printf("extract_ports6: can't locate header! ip6=%p\n", ip6); 1004 #endif 1005 return 0; 1006 } 1007 off = ((char *)ip6 - m0->m_data) + sizeof(struct ip6_hdr); 1008 1009 proto = ip6->ip6_nxt; 1010 do { 1011 while (off >= m0->m_len) { 1012 off -= m0->m_len; 1013 m0 = m0->m_next; 1014 if (m0 == NULL) 1015 return 0; 1016 } 1017 if (m0->m_len < off + 4) 1018 return 0; 1019 1020 switch (proto) { 1021 case IPPROTO_TCP: 1022 case IPPROTO_UDP: { 1023 struct udphdr *udp; 1024 1025 udp = (struct udphdr *)(mtod(m0, char *) + off); 1026 fin6->fi6_sport = udp->uh_sport; 1027 fin6->fi6_dport = udp->uh_dport; 1028 fin6->fi6_proto = proto; 1029 } 1030 return 1; 1031 1032 case IPPROTO_ESP: 1033 if (fin6->fi6_gpi == 0) { 1034 u_int32_t *gpi; 1035 1036 gpi = (u_int32_t *)(mtod(m0, char *) + off); 1037 fin6->fi6_gpi = *gpi; 1038 } 1039 fin6->fi6_proto = proto; 1040 return 1; 1041 1042 case IPPROTO_AH: { 1043 /* get next header and header length */ 1044 struct _opt6 *opt6; 1045 1046 opt6 = (struct _opt6 *)(mtod(m0, char *) + off); 1047 if (fin6->fi6_gpi == 0 && m0->m_len >= off + 8) 1048 fin6->fi6_gpi = opt6->ah_spi; 1049 proto = opt6->opt6_nxt; 1050 off += 8 + (opt6->opt6_hlen * 4); 1051 /* goto the next header */ 1052 break; 1053 } 1054 1055 case IPPROTO_HOPOPTS: 1056 case IPPROTO_ROUTING: 1057 case IPPROTO_DSTOPTS: { 1058 /* get next header and header length */ 1059 struct _opt6 *opt6; 1060 1061 opt6 = (struct _opt6 *)(mtod(m0, char *) + off); 1062 proto = opt6->opt6_nxt; 1063 off += (opt6->opt6_hlen + 1) * 8; 1064 /* goto the next header */ 1065 break; 1066 } 1067 1068 case IPPROTO_FRAGMENT: 1069 /* ipv6 fragmentations are not supported yet */ 1070 default: 1071 fin6->fi6_proto = proto; 1072 return 0; 1073 } 1074 } while (1); 1075 /*NOTREACHED*/ 1076 } 1077 #endif /* INET6 */ 1078 1079 /* 1080 * altq common classifier 1081 */ 1082 int 1083 acc_add_filter(struct acc_classifier *classifier, struct flow_filter *filter, 1084 void *class, u_long *phandle) 1085 { 1086 struct acc_filter *afp, *prev, *tmp; 1087 int i, s; 1088 1089 #ifdef INET6 1090 if (filter->ff_flow.fi_family != AF_INET && 1091 filter->ff_flow.fi_family != AF_INET6) 1092 return EINVAL; 1093 #else 1094 if (filter->ff_flow.fi_family != AF_INET) 1095 return EINVAL; 1096 #endif 1097 1098 afp = malloc(sizeof(struct acc_filter), M_DEVBUF, M_WAITOK|M_ZERO); 1099 if (afp == NULL) 1100 return ENOMEM; 1101 1102 afp->f_filter = *filter; 1103 afp->f_class = class; 1104 1105 i = ACC_WILDCARD_INDEX; 1106 if (filter->ff_flow.fi_family == AF_INET) { 1107 struct flow_filter *filter4 = &afp->f_filter; 1108 1109 /* 1110 * if address is 0, it's a wildcard. if address mask 1111 * isn't set, use full mask. 1112 */ 1113 if (filter4->ff_flow.fi_dst.s_addr == 0) 1114 filter4->ff_mask.mask_dst.s_addr = 0; 1115 else if (filter4->ff_mask.mask_dst.s_addr == 0) 1116 filter4->ff_mask.mask_dst.s_addr = 0xffffffff; 1117 if (filter4->ff_flow.fi_src.s_addr == 0) 1118 filter4->ff_mask.mask_src.s_addr = 0; 1119 else if (filter4->ff_mask.mask_src.s_addr == 0) 1120 filter4->ff_mask.mask_src.s_addr = 0xffffffff; 1121 1122 /* clear extra bits in addresses */ 1123 filter4->ff_flow.fi_dst.s_addr &= 1124 filter4->ff_mask.mask_dst.s_addr; 1125 filter4->ff_flow.fi_src.s_addr &= 1126 filter4->ff_mask.mask_src.s_addr; 1127 1128 /* 1129 * if dst address is a wildcard, use hash-entry 1130 * ACC_WILDCARD_INDEX. 1131 */ 1132 if (filter4->ff_mask.mask_dst.s_addr != 0xffffffff) 1133 i = ACC_WILDCARD_INDEX; 1134 else 1135 i = ACC_GET_HASH_INDEX(filter4->ff_flow.fi_dst.s_addr); 1136 } 1137 #ifdef INET6 1138 else if (filter->ff_flow.fi_family == AF_INET6) { 1139 struct flow_filter6 *filter6 = 1140 (struct flow_filter6 *)&afp->f_filter; 1141 #ifndef IN6MASK0 /* taken from kame ipv6 */ 1142 #define IN6MASK0 {{{ 0, 0, 0, 0 }}} 1143 #define IN6MASK128 {{{ 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff }}} 1144 const struct in6_addr in6mask0 = IN6MASK0; 1145 const struct in6_addr in6mask128 = IN6MASK128; 1146 #endif 1147 1148 if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_flow6.fi6_dst)) 1149 filter6->ff_mask6.mask6_dst = in6mask0; 1150 else if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_mask6.mask6_dst)) 1151 filter6->ff_mask6.mask6_dst = in6mask128; 1152 if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_flow6.fi6_src)) 1153 filter6->ff_mask6.mask6_src = in6mask0; 1154 else if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_mask6.mask6_src)) 1155 filter6->ff_mask6.mask6_src = in6mask128; 1156 1157 /* clear extra bits in addresses */ 1158 for (i = 0; i < 16; i++) 1159 filter6->ff_flow6.fi6_dst.s6_addr[i] &= 1160 filter6->ff_mask6.mask6_dst.s6_addr[i]; 1161 for (i = 0; i < 16; i++) 1162 filter6->ff_flow6.fi6_src.s6_addr[i] &= 1163 filter6->ff_mask6.mask6_src.s6_addr[i]; 1164 1165 if (filter6->ff_flow6.fi6_flowlabel == 0) 1166 i = ACC_WILDCARD_INDEX; 1167 else 1168 i = ACC_GET_HASH_INDEX(filter6->ff_flow6.fi6_flowlabel); 1169 } 1170 #endif /* INET6 */ 1171 1172 afp->f_handle = get_filt_handle(classifier, i); 1173 1174 /* update filter bitmask */ 1175 afp->f_fbmask = filt2fibmask(filter); 1176 classifier->acc_fbmask |= afp->f_fbmask; 1177 1178 /* 1179 * add this filter to the filter list. 1180 * filters are ordered from the highest rule number. 1181 */ 1182 s = splnet(); 1183 prev = NULL; 1184 LIST_FOREACH(tmp, &classifier->acc_filters[i], f_chain) { 1185 if (tmp->f_filter.ff_ruleno > afp->f_filter.ff_ruleno) 1186 prev = tmp; 1187 else 1188 break; 1189 } 1190 if (prev == NULL) 1191 LIST_INSERT_HEAD(&classifier->acc_filters[i], afp, f_chain); 1192 else 1193 LIST_INSERT_AFTER(prev, afp, f_chain); 1194 splx(s); 1195 1196 *phandle = afp->f_handle; 1197 return 0; 1198 } 1199 1200 int 1201 acc_delete_filter(struct acc_classifier *classifier, u_long handle) 1202 { 1203 struct acc_filter *afp; 1204 int s; 1205 1206 if ((afp = filth_to_filtp(classifier, handle)) == NULL) 1207 return EINVAL; 1208 1209 s = splnet(); 1210 LIST_REMOVE(afp, f_chain); 1211 splx(s); 1212 1213 free(afp, M_DEVBUF); 1214 1215 /* todo: update filt_bmask */ 1216 1217 return 0; 1218 } 1219 1220 /* 1221 * delete filters referencing to the specified class. 1222 * if the all flag is not 0, delete all the filters. 1223 */ 1224 int 1225 acc_discard_filters(struct acc_classifier *classifier, void *class, int all) 1226 { 1227 struct acc_filter *afp; 1228 int i, s; 1229 1230 s = splnet(); 1231 for (i = 0; i < ACC_FILTER_TABLESIZE; i++) { 1232 do { 1233 LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain) 1234 if (all || afp->f_class == class) { 1235 LIST_REMOVE(afp, f_chain); 1236 free(afp, M_DEVBUF); 1237 /* start again from the head */ 1238 break; 1239 } 1240 } while (afp != NULL); 1241 } 1242 splx(s); 1243 1244 if (all) 1245 classifier->acc_fbmask = 0; 1246 1247 return 0; 1248 } 1249 1250 void * 1251 acc_classify(void *clfier, struct mbuf *m, int af) 1252 { 1253 struct acc_classifier *classifier; 1254 struct flowinfo flow; 1255 struct acc_filter *afp; 1256 int i; 1257 1258 classifier = (struct acc_classifier *)clfier; 1259 altq_extractflow(m, af, &flow, classifier->acc_fbmask); 1260 1261 if (flow.fi_family == AF_INET) { 1262 struct flowinfo_in *fp = (struct flowinfo_in *)&flow; 1263 1264 if ((classifier->acc_fbmask & FIMB4_ALL) == FIMB4_TOS) { 1265 /* only tos is used */ 1266 LIST_FOREACH(afp, 1267 &classifier->acc_filters[ACC_WILDCARD_INDEX], 1268 f_chain) 1269 if (apply_tosfilter4(afp->f_fbmask, 1270 &afp->f_filter, fp)) 1271 /* filter matched */ 1272 return (afp->f_class); 1273 } else if ((classifier->acc_fbmask & 1274 (~(FIMB4_PROTO|FIMB4_SPORT|FIMB4_DPORT) & FIMB4_ALL)) 1275 == 0) { 1276 /* only proto and ports are used */ 1277 LIST_FOREACH(afp, 1278 &classifier->acc_filters[ACC_WILDCARD_INDEX], 1279 f_chain) 1280 if (apply_ppfilter4(afp->f_fbmask, 1281 &afp->f_filter, fp)) 1282 /* filter matched */ 1283 return (afp->f_class); 1284 } else { 1285 /* get the filter hash entry from its dest address */ 1286 i = ACC_GET_HASH_INDEX(fp->fi_dst.s_addr); 1287 do { 1288 /* 1289 * go through this loop twice. first for dst 1290 * hash, second for wildcards. 1291 */ 1292 LIST_FOREACH(afp, &classifier->acc_filters[i], 1293 f_chain) 1294 if (apply_filter4(afp->f_fbmask, 1295 &afp->f_filter, fp)) 1296 /* filter matched */ 1297 return (afp->f_class); 1298 1299 /* 1300 * check again for filters with a dst addr 1301 * wildcard. 1302 * (daddr == 0 || dmask != 0xffffffff). 1303 */ 1304 if (i != ACC_WILDCARD_INDEX) 1305 i = ACC_WILDCARD_INDEX; 1306 else 1307 break; 1308 } while (1); 1309 } 1310 } 1311 #ifdef INET6 1312 else if (flow.fi_family == AF_INET6) { 1313 struct flowinfo_in6 *fp6 = (struct flowinfo_in6 *)&flow; 1314 1315 /* get the filter hash entry from its flow ID */ 1316 if (fp6->fi6_flowlabel != 0) 1317 i = ACC_GET_HASH_INDEX(fp6->fi6_flowlabel); 1318 else 1319 /* flowlable can be zero */ 1320 i = ACC_WILDCARD_INDEX; 1321 1322 /* go through this loop twice. first for flow hash, second 1323 for wildcards. */ 1324 do { 1325 LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain) 1326 if (apply_filter6(afp->f_fbmask, 1327 (struct flow_filter6 *)&afp->f_filter, 1328 fp6)) 1329 /* filter matched */ 1330 return (afp->f_class); 1331 1332 /* 1333 * check again for filters with a wildcard. 1334 */ 1335 if (i != ACC_WILDCARD_INDEX) 1336 i = ACC_WILDCARD_INDEX; 1337 else 1338 break; 1339 } while (1); 1340 } 1341 #endif /* INET6 */ 1342 1343 /* no filter matched */ 1344 return NULL; 1345 } 1346 1347 static int 1348 apply_filter4(u_int32_t fbmask, struct flow_filter *filt, 1349 struct flowinfo_in *pkt) 1350 { 1351 if (filt->ff_flow.fi_family != AF_INET) 1352 return 0; 1353 if ((fbmask & FIMB4_SPORT) && filt->ff_flow.fi_sport != pkt->fi_sport) 1354 return 0; 1355 if ((fbmask & FIMB4_DPORT) && filt->ff_flow.fi_dport != pkt->fi_dport) 1356 return 0; 1357 if ((fbmask & FIMB4_DADDR) && 1358 filt->ff_flow.fi_dst.s_addr != 1359 (pkt->fi_dst.s_addr & filt->ff_mask.mask_dst.s_addr)) 1360 return 0; 1361 if ((fbmask & FIMB4_SADDR) && 1362 filt->ff_flow.fi_src.s_addr != 1363 (pkt->fi_src.s_addr & filt->ff_mask.mask_src.s_addr)) 1364 return 0; 1365 if ((fbmask & FIMB4_PROTO) && filt->ff_flow.fi_proto != pkt->fi_proto) 1366 return 0; 1367 if ((fbmask & FIMB4_TOS) && filt->ff_flow.fi_tos != 1368 (pkt->fi_tos & filt->ff_mask.mask_tos)) 1369 return 0; 1370 if ((fbmask & FIMB4_GPI) && filt->ff_flow.fi_gpi != (pkt->fi_gpi)) 1371 return 0; 1372 /* match */ 1373 return 1; 1374 } 1375 1376 /* 1377 * filter matching function optimized for a common case that checks 1378 * only protocol and port numbers 1379 */ 1380 static int 1381 apply_ppfilter4(u_int32_t fbmask, struct flow_filter *filt, 1382 struct flowinfo_in *pkt) 1383 { 1384 if (filt->ff_flow.fi_family != AF_INET) 1385 return 0; 1386 if ((fbmask & FIMB4_SPORT) && filt->ff_flow.fi_sport != pkt->fi_sport) 1387 return 0; 1388 if ((fbmask & FIMB4_DPORT) && filt->ff_flow.fi_dport != pkt->fi_dport) 1389 return 0; 1390 if ((fbmask & FIMB4_PROTO) && filt->ff_flow.fi_proto != pkt->fi_proto) 1391 return 0; 1392 /* match */ 1393 return 1; 1394 } 1395 1396 /* 1397 * filter matching function only for tos field. 1398 */ 1399 static int 1400 apply_tosfilter4(u_int32_t fbmask, struct flow_filter *filt, 1401 struct flowinfo_in *pkt) 1402 { 1403 if (filt->ff_flow.fi_family != AF_INET) 1404 return 0; 1405 if ((fbmask & FIMB4_TOS) && filt->ff_flow.fi_tos != 1406 (pkt->fi_tos & filt->ff_mask.mask_tos)) 1407 return 0; 1408 /* match */ 1409 return 1; 1410 } 1411 1412 #ifdef INET6 1413 static int 1414 apply_filter6(u_int32_t fbmask, struct flow_filter6 *filt, 1415 struct flowinfo_in6 *pkt) 1416 { 1417 int i; 1418 1419 if (filt->ff_flow6.fi6_family != AF_INET6) 1420 return 0; 1421 if ((fbmask & FIMB6_FLABEL) && 1422 filt->ff_flow6.fi6_flowlabel != pkt->fi6_flowlabel) 1423 return 0; 1424 if ((fbmask & FIMB6_PROTO) && 1425 filt->ff_flow6.fi6_proto != pkt->fi6_proto) 1426 return 0; 1427 if ((fbmask & FIMB6_SPORT) && 1428 filt->ff_flow6.fi6_sport != pkt->fi6_sport) 1429 return 0; 1430 if ((fbmask & FIMB6_DPORT) && 1431 filt->ff_flow6.fi6_dport != pkt->fi6_dport) 1432 return 0; 1433 if (fbmask & FIMB6_SADDR) { 1434 for (i = 0; i < 4; i++) 1435 if (filt->ff_flow6.fi6_src.s6_addr32[i] != 1436 (pkt->fi6_src.s6_addr32[i] & 1437 filt->ff_mask6.mask6_src.s6_addr32[i])) 1438 return 0; 1439 } 1440 if (fbmask & FIMB6_DADDR) { 1441 for (i = 0; i < 4; i++) 1442 if (filt->ff_flow6.fi6_dst.s6_addr32[i] != 1443 (pkt->fi6_dst.s6_addr32[i] & 1444 filt->ff_mask6.mask6_dst.s6_addr32[i])) 1445 return 0; 1446 } 1447 if ((fbmask & FIMB6_TCLASS) && 1448 filt->ff_flow6.fi6_tclass != 1449 (pkt->fi6_tclass & filt->ff_mask6.mask6_tclass)) 1450 return 0; 1451 if ((fbmask & FIMB6_GPI) && 1452 filt->ff_flow6.fi6_gpi != pkt->fi6_gpi) 1453 return 0; 1454 /* match */ 1455 return 1; 1456 } 1457 #endif /* INET6 */ 1458 1459 /* 1460 * filter handle: 1461 * bit 20-28: index to the filter hash table 1462 * bit 0-19: unique id in the hash bucket. 1463 */ 1464 static u_long 1465 get_filt_handle(struct acc_classifier *classifier, int i) 1466 { 1467 static u_long handle_number = 1; 1468 u_long handle; 1469 struct acc_filter *afp; 1470 1471 while (1) { 1472 handle = handle_number++ & 0x000fffff; 1473 1474 if (LIST_EMPTY(&classifier->acc_filters[i])) 1475 break; 1476 1477 LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain) 1478 if ((afp->f_handle & 0x000fffff) == handle) 1479 break; 1480 if (afp == NULL) 1481 break; 1482 /* this handle is already used, try again */ 1483 } 1484 1485 return ((i << 20) | handle); 1486 } 1487 1488 /* convert filter handle to filter pointer */ 1489 static struct acc_filter * 1490 filth_to_filtp(struct acc_classifier *classifier, u_long handle) 1491 { 1492 struct acc_filter *afp; 1493 int i; 1494 1495 i = ACC_GET_HINDEX(handle); 1496 1497 LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain) 1498 if (afp->f_handle == handle) 1499 return afp; 1500 1501 return NULL; 1502 } 1503 1504 /* create flowinfo bitmask */ 1505 static u_int32_t 1506 filt2fibmask(struct flow_filter *filt) 1507 { 1508 u_int32_t mask = 0; 1509 #ifdef INET6 1510 struct flow_filter6 *filt6; 1511 #endif 1512 1513 switch (filt->ff_flow.fi_family) { 1514 case AF_INET: 1515 if (filt->ff_flow.fi_proto != 0) 1516 mask |= FIMB4_PROTO; 1517 if (filt->ff_flow.fi_tos != 0) 1518 mask |= FIMB4_TOS; 1519 if (filt->ff_flow.fi_dst.s_addr != 0) 1520 mask |= FIMB4_DADDR; 1521 if (filt->ff_flow.fi_src.s_addr != 0) 1522 mask |= FIMB4_SADDR; 1523 if (filt->ff_flow.fi_sport != 0) 1524 mask |= FIMB4_SPORT; 1525 if (filt->ff_flow.fi_dport != 0) 1526 mask |= FIMB4_DPORT; 1527 if (filt->ff_flow.fi_gpi != 0) 1528 mask |= FIMB4_GPI; 1529 break; 1530 #ifdef INET6 1531 case AF_INET6: 1532 filt6 = (struct flow_filter6 *)filt; 1533 1534 if (filt6->ff_flow6.fi6_proto != 0) 1535 mask |= FIMB6_PROTO; 1536 if (filt6->ff_flow6.fi6_tclass != 0) 1537 mask |= FIMB6_TCLASS; 1538 if (!IN6_IS_ADDR_UNSPECIFIED(&filt6->ff_flow6.fi6_dst)) 1539 mask |= FIMB6_DADDR; 1540 if (!IN6_IS_ADDR_UNSPECIFIED(&filt6->ff_flow6.fi6_src)) 1541 mask |= FIMB6_SADDR; 1542 if (filt6->ff_flow6.fi6_sport != 0) 1543 mask |= FIMB6_SPORT; 1544 if (filt6->ff_flow6.fi6_dport != 0) 1545 mask |= FIMB6_DPORT; 1546 if (filt6->ff_flow6.fi6_gpi != 0) 1547 mask |= FIMB6_GPI; 1548 if (filt6->ff_flow6.fi6_flowlabel != 0) 1549 mask |= FIMB6_FLABEL; 1550 break; 1551 #endif /* INET6 */ 1552 } 1553 return mask; 1554 } 1555 1556 1557 /* 1558 * helper functions to handle IPv4 fragments. 1559 * currently only in-sequence fragments are handled. 1560 * - fragment info is cached in a LRU list. 1561 * - when a first fragment is found, cache its flow info. 1562 * - when a non-first fragment is found, lookup the cache. 1563 */ 1564 1565 struct ip4_frag { 1566 TAILQ_ENTRY(ip4_frag) ip4f_chain; 1567 char ip4f_valid; 1568 u_short ip4f_id; 1569 struct flowinfo_in ip4f_info; 1570 }; 1571 1572 static TAILQ_HEAD(ip4f_list, ip4_frag) ip4f_list; /* IPv4 fragment cache */ 1573 1574 #define IP4F_TABSIZE 16 /* IPv4 fragment cache size */ 1575 1576 1577 static void 1578 ip4f_cache(struct ip *ip, struct flowinfo_in *fin) 1579 { 1580 struct ip4_frag *fp; 1581 1582 if (TAILQ_EMPTY(&ip4f_list)) { 1583 /* first time call, allocate fragment cache entries. */ 1584 if (ip4f_init() < 0) 1585 /* allocation failed! */ 1586 return; 1587 } 1588 1589 fp = ip4f_alloc(); 1590 fp->ip4f_id = ip->ip_id; 1591 fp->ip4f_info.fi_proto = ip->ip_p; 1592 fp->ip4f_info.fi_src.s_addr = ip->ip_src.s_addr; 1593 fp->ip4f_info.fi_dst.s_addr = ip->ip_dst.s_addr; 1594 1595 /* save port numbers */ 1596 fp->ip4f_info.fi_sport = fin->fi_sport; 1597 fp->ip4f_info.fi_dport = fin->fi_dport; 1598 fp->ip4f_info.fi_gpi = fin->fi_gpi; 1599 } 1600 1601 static int 1602 ip4f_lookup(struct ip *ip, struct flowinfo_in *fin) 1603 { 1604 struct ip4_frag *fp; 1605 1606 for (fp = TAILQ_FIRST(&ip4f_list); fp != NULL && fp->ip4f_valid; 1607 fp = TAILQ_NEXT(fp, ip4f_chain)) 1608 if (ip->ip_id == fp->ip4f_id && 1609 ip->ip_src.s_addr == fp->ip4f_info.fi_src.s_addr && 1610 ip->ip_dst.s_addr == fp->ip4f_info.fi_dst.s_addr && 1611 ip->ip_p == fp->ip4f_info.fi_proto) { 1612 1613 /* found the matching entry */ 1614 fin->fi_sport = fp->ip4f_info.fi_sport; 1615 fin->fi_dport = fp->ip4f_info.fi_dport; 1616 fin->fi_gpi = fp->ip4f_info.fi_gpi; 1617 1618 if ((ntohs(ip->ip_off) & IP_MF) == 0) 1619 /* this is the last fragment, 1620 release the entry. */ 1621 ip4f_free(fp); 1622 1623 return 1; 1624 } 1625 1626 /* no matching entry found */ 1627 return 0; 1628 } 1629 1630 static int 1631 ip4f_init(void) 1632 { 1633 struct ip4_frag *fp; 1634 int i; 1635 1636 TAILQ_INIT(&ip4f_list); 1637 for (i=0; i<IP4F_TABSIZE; i++) { 1638 fp = malloc(sizeof(struct ip4_frag), M_DEVBUF, M_NOWAIT); 1639 if (fp == NULL) { 1640 printf("ip4f_init: can't alloc %dth entry!\n", i); 1641 if (i == 0) 1642 return -1; 1643 return 0; 1644 } 1645 fp->ip4f_valid = 0; 1646 TAILQ_INSERT_TAIL(&ip4f_list, fp, ip4f_chain); 1647 } 1648 return 0; 1649 } 1650 1651 static struct ip4_frag * 1652 ip4f_alloc(void) 1653 { 1654 struct ip4_frag *fp; 1655 1656 /* reclaim an entry at the tail, put it at the head */ 1657 fp = TAILQ_LAST(&ip4f_list, ip4f_list); 1658 TAILQ_REMOVE(&ip4f_list, fp, ip4f_chain); 1659 fp->ip4f_valid = 1; 1660 TAILQ_INSERT_HEAD(&ip4f_list, fp, ip4f_chain); 1661 return fp; 1662 } 1663 1664 static void 1665 ip4f_free(struct ip4_frag *fp) 1666 { 1667 TAILQ_REMOVE(&ip4f_list, fp, ip4f_chain); 1668 fp->ip4f_valid = 0; 1669 TAILQ_INSERT_TAIL(&ip4f_list, fp, ip4f_chain); 1670 } 1671 1672 #endif /* ALTQ3_CLFIER_COMPAT */ 1673
Indexes created Sun Sep 21 20:09:37 GMT 2025