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