1/* $NetBSD: altq_subr.c,v 1.33 2017/03/14 09:03:08 ozaki-r 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.33 2017/03/14 09:03:08 ozaki-r 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 */ 76static void tbr_timeout(void *); 77int (*altq_input)(struct mbuf *, int) = NULL; 78static int tbr_timer = 0; /* token bucket regulator timer */ 79static struct callout tbr_callout; 80 81#ifdef ALTQ3_CLFIER_COMPAT 82static int extract_ports4(struct mbuf *, struct ip *, struct flowinfo_in *); 83#ifdef INET6 84static int extract_ports6(struct mbuf *, struct ip6_hdr *, 85 struct flowinfo_in6 *); 86#endif 87static int apply_filter4(u_int32_t, struct flow_filter *, 88 struct flowinfo_in *); 89static int apply_ppfilter4(u_int32_t, struct flow_filter *, 90 struct flowinfo_in *); 91#ifdef INET6 92static int apply_filter6(u_int32_t, struct flow_filter6 *, 93 struct flowinfo_in6 *); 94#endif 95static int apply_tosfilter4(u_int32_t, struct flow_filter *, 96 struct flowinfo_in *); 97static u_long get_filt_handle(struct acc_classifier *, int); 98static struct acc_filter *filth_to_filtp(struct acc_classifier *, u_long); 99static u_int32_t filt2fibmask(struct flow_filter *); 100 101static void ip4f_cache(struct ip *, struct flowinfo_in *); 102static int ip4f_lookup(struct ip *, struct flowinfo_in *); 103static int ip4f_init(void); 104static struct ip4_frag *ip4f_alloc(void); 105static 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. */ 113void * 114altq_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 126int 127altq_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 164int 165altq_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 190int 191altq_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 211int 212altq_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 228void 229altq_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 249struct mbuf * 250tbr_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 */ 298int 299tbr_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 */ 351static void 352tbr_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 */ 387int 388tbr_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 */ 408int 409altq_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 */ 443int 444altq_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 */ 469int 470altq_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 */ 508int 509altq_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 */ 542int 543altq_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 */ 573int 574altq_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 */ 604int 605altq_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 */ 636u_int8_t 637read_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 681void 682write_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 744u_int32_t machclk_freq = 0; 745u_int32_t machclk_per_tick = 0; 746 747void 748init_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 766u_int64_t 767read_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 */ 792int 793altq_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 */ 879struct _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 */ 890static int 891extract_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 985static int 986extract_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 */ 1082int 1083acc_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 1200int 1201acc_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 */ 1224int 1225acc_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 1250void * 1251acc_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 1347static int 1348apply_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 */ 1380static int 1381apply_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 */ 1399static int 1400apply_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 1413static int 1414apply_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 */ 1464static u_long 1465get_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 */ 1489static struct acc_filter * 1490filth_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 */ 1505static u_int32_t 1506filt2fibmask(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 1565struct 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 1572static TAILQ_HEAD(ip4f_list, ip4_frag) ip4f_list; /* IPv4 fragment cache */ 1573 1574#define IP4F_TABSIZE 16 /* IPv4 fragment cache size */ 1575 1576 1577static void 1578ip4f_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 1601static int 1602ip4f_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 1630static int 1631ip4f_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 1651static struct ip4_frag * 1652ip4f_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 1664static void 1665ip4f_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