1 /* $KAME: altq_subr.c,v 1.23 2004/04/20 16:10:06 itojun Exp $ */ 2 /* $DragonFly: src/sys/net/altq/altq_subr.c,v 1.12 2008/05/14 11:59:23 sephe 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 "opt_altq.h" 31 #include "opt_inet.h" 32 #include "opt_inet6.h" 33 34 #include <sys/param.h> 35 #include <sys/malloc.h> 36 #include <sys/mbuf.h> 37 #include <sys/systm.h> 38 #include <sys/proc.h> 39 #include <sys/socket.h> 40 #include <sys/socketvar.h> 41 #include <sys/kernel.h> 42 #include <sys/callout.h> 43 #include <sys/errno.h> 44 #include <sys/syslog.h> 45 #include <sys/sysctl.h> 46 #include <sys/queue.h> 47 #include <sys/thread2.h> 48 49 #include <net/if.h> 50 #include <net/if_dl.h> 51 #include <net/if_types.h> 52 #include <net/ifq_var.h> 53 54 #include <netinet/in.h> 55 #include <netinet/in_systm.h> 56 #include <netinet/ip.h> 57 #ifdef INET6 58 #include <netinet/ip6.h> 59 #endif 60 #include <netinet/tcp.h> 61 #include <netinet/udp.h> 62 63 #include <net/pf/pfvar.h> 64 #include <net/altq/altq.h> 65 66 /* machine dependent clock related includes */ 67 #include <machine/clock.h> /* for tsc_frequency */ 68 #include <machine/md_var.h> /* for cpu_feature */ 69 #include <machine/specialreg.h> /* for CPUID_TSC */ 70 71 /* 72 * internal function prototypes 73 */ 74 static void tbr_timeout(void *); 75 static int altq_enable_locked(struct ifaltq *); 76 static int altq_disable_locked(struct ifaltq *); 77 static int altq_detach_locked(struct ifaltq *); 78 static int tbr_set_locked(struct ifaltq *, struct tb_profile *); 79 80 int (*altq_input)(struct mbuf *, int) = NULL; 81 static int tbr_timer = 0; /* token bucket regulator timer */ 82 static struct callout tbr_callout; 83 84 int pfaltq_running; /* keep track of running state */ 85 86 MALLOC_DEFINE(M_ALTQ, "altq", "ALTQ structures"); 87 88 /* 89 * alternate queueing support routines 90 */ 91 92 /* look up the queue state by the interface name and the queueing type. */ 93 void * 94 altq_lookup(const char *name, int type) 95 { 96 struct ifnet *ifp; 97 98 if ((ifp = ifunit(name)) != NULL) { 99 if (type != ALTQT_NONE && ifp->if_snd.altq_type == type) 100 return (ifp->if_snd.altq_disc); 101 } 102 103 return (NULL); 104 } 105 106 int 107 altq_attach(struct ifaltq *ifq, int type, void *discipline, 108 int (*enqueue)(struct ifaltq *, struct mbuf *, struct altq_pktattr *), 109 struct mbuf *(*dequeue)(struct ifaltq *, struct mbuf *, int), 110 int (*request)(struct ifaltq *, int, void *), 111 void *clfier, 112 void *(*classify)(struct ifaltq *, struct mbuf *, 113 struct altq_pktattr *)) 114 { 115 if (!ifq_is_ready(ifq)) 116 return ENXIO; 117 118 ifq->altq_type = type; 119 ifq->altq_disc = discipline; 120 ifq->altq_enqueue = enqueue; 121 ifq->altq_dequeue = dequeue; 122 ifq->altq_request = request; 123 ifq->altq_clfier = clfier; 124 ifq->altq_classify = classify; 125 ifq->altq_flags &= (ALTQF_CANTCHANGE|ALTQF_ENABLED); 126 return 0; 127 } 128 129 static int 130 altq_detach_locked(struct ifaltq *ifq) 131 { 132 if (!ifq_is_ready(ifq)) 133 return ENXIO; 134 if (ifq_is_enabled(ifq)) 135 return EBUSY; 136 if (!ifq_is_attached(ifq)) 137 return (0); 138 139 ifq_set_classic(ifq); 140 ifq->altq_type = ALTQT_NONE; 141 ifq->altq_disc = NULL; 142 ifq->altq_clfier = NULL; 143 ifq->altq_classify = NULL; 144 ifq->altq_flags &= ALTQF_CANTCHANGE; 145 return 0; 146 } 147 148 int 149 altq_detach(struct ifaltq *ifq) 150 { 151 int error; 152 153 ALTQ_LOCK(ifq); 154 error = altq_detach_locked(ifq); 155 ALTQ_UNLOCK(ifq); 156 return error; 157 } 158 159 static int 160 altq_enable_locked(struct ifaltq *ifq) 161 { 162 if (!ifq_is_ready(ifq)) 163 return ENXIO; 164 if (ifq_is_enabled(ifq)) 165 return 0; 166 167 ifq_purge_locked(ifq); 168 KKASSERT(ifq->ifq_len == 0); 169 170 ifq->altq_flags |= ALTQF_ENABLED; 171 if (ifq->altq_clfier != NULL) 172 ifq->altq_flags |= ALTQF_CLASSIFY; 173 return 0; 174 } 175 176 int 177 altq_enable(struct ifaltq *ifq) 178 { 179 int error; 180 181 ALTQ_LOCK(ifq); 182 error = altq_enable_locked(ifq); 183 ALTQ_UNLOCK(ifq); 184 return error; 185 } 186 187 static int 188 altq_disable_locked(struct ifaltq *ifq) 189 { 190 if (!ifq_is_enabled(ifq)) 191 return 0; 192 193 ifq_purge_locked(ifq); 194 KKASSERT(ifq->ifq_len == 0); 195 ifq->altq_flags &= ~(ALTQF_ENABLED|ALTQF_CLASSIFY); 196 return 0; 197 } 198 199 int 200 altq_disable(struct ifaltq *ifq) 201 { 202 int error; 203 204 ALTQ_LOCK(ifq); 205 error = altq_disable_locked(ifq); 206 ALTQ_UNLOCK(ifq); 207 return error; 208 } 209 210 /* 211 * internal representation of token bucket parameters 212 * rate: byte_per_unittime << 32 213 * (((bits_per_sec) / 8) << 32) / machclk_freq 214 * depth: byte << 32 215 * 216 */ 217 #define TBR_SHIFT 32 218 #define TBR_SCALE(x) ((int64_t)(x) << TBR_SHIFT) 219 #define TBR_UNSCALE(x) ((x) >> TBR_SHIFT) 220 221 struct mbuf * 222 tbr_dequeue(struct ifaltq *ifq, struct mbuf *mpolled, int op) 223 { 224 struct tb_regulator *tbr; 225 struct mbuf *m; 226 int64_t interval; 227 uint64_t now; 228 229 crit_enter(); 230 tbr = ifq->altq_tbr; 231 if (op == ALTDQ_REMOVE && tbr->tbr_lastop == ALTDQ_POLL) { 232 /* if this is a remove after poll, bypass tbr check */ 233 } else { 234 /* update token only when it is negative */ 235 if (tbr->tbr_token <= 0) { 236 now = read_machclk(); 237 interval = now - tbr->tbr_last; 238 if (interval >= tbr->tbr_filluptime) 239 tbr->tbr_token = tbr->tbr_depth; 240 else { 241 tbr->tbr_token += interval * tbr->tbr_rate; 242 if (tbr->tbr_token > tbr->tbr_depth) 243 tbr->tbr_token = tbr->tbr_depth; 244 } 245 tbr->tbr_last = now; 246 } 247 /* if token is still negative, don't allow dequeue */ 248 if (tbr->tbr_token <= 0) { 249 crit_exit(); 250 return (NULL); 251 } 252 } 253 254 if (ifq_is_enabled(ifq)) { 255 m = (*ifq->altq_dequeue)(ifq, mpolled, op); 256 } else if (op == ALTDQ_POLL) { 257 IF_POLL(ifq, m); 258 } else { 259 IF_DEQUEUE(ifq, m); 260 KKASSERT(mpolled == NULL || mpolled == m); 261 } 262 263 if (m != NULL && op == ALTDQ_REMOVE) 264 tbr->tbr_token -= TBR_SCALE(m_pktlen(m)); 265 tbr->tbr_lastop = op; 266 crit_exit(); 267 return (m); 268 } 269 270 /* 271 * set a token bucket regulator. 272 * if the specified rate is zero, the token bucket regulator is deleted. 273 */ 274 static int 275 tbr_set_locked(struct ifaltq *ifq, struct tb_profile *profile) 276 { 277 struct tb_regulator *tbr, *otbr; 278 279 if (machclk_freq == 0) 280 init_machclk(); 281 if (machclk_freq == 0) { 282 kprintf("%s: no cpu clock available!\n", __func__); 283 return (ENXIO); 284 } 285 286 if (profile->rate == 0) { 287 /* delete this tbr */ 288 if ((tbr = ifq->altq_tbr) == NULL) 289 return (ENOENT); 290 ifq->altq_tbr = NULL; 291 kfree(tbr, M_ALTQ); 292 return (0); 293 } 294 295 tbr = kmalloc(sizeof(*tbr), M_ALTQ, M_WAITOK | M_ZERO); 296 tbr->tbr_rate = TBR_SCALE(profile->rate / 8) / machclk_freq; 297 tbr->tbr_depth = TBR_SCALE(profile->depth); 298 if (tbr->tbr_rate > 0) 299 tbr->tbr_filluptime = tbr->tbr_depth / tbr->tbr_rate; 300 else 301 tbr->tbr_filluptime = 0xffffffffffffffffLL; 302 tbr->tbr_token = tbr->tbr_depth; 303 tbr->tbr_last = read_machclk(); 304 tbr->tbr_lastop = ALTDQ_REMOVE; 305 306 otbr = ifq->altq_tbr; 307 ifq->altq_tbr = tbr; /* set the new tbr */ 308 309 if (otbr != NULL) 310 kfree(otbr, M_ALTQ); 311 else if (tbr_timer == 0) { 312 callout_reset(&tbr_callout, 1, tbr_timeout, NULL); 313 tbr_timer = 1; 314 } 315 return (0); 316 } 317 318 int 319 tbr_set(struct ifaltq *ifq, struct tb_profile *profile) 320 { 321 int error; 322 323 ALTQ_LOCK(ifq); 324 error = tbr_set_locked(ifq, profile); 325 ALTQ_UNLOCK(ifq); 326 return error; 327 } 328 329 /* 330 * tbr_timeout goes through the interface list, and kicks the drivers 331 * if necessary. 332 */ 333 static void 334 tbr_timeout(void *arg) 335 { 336 struct ifnet *ifp; 337 int active; 338 339 active = 0; 340 crit_enter(); 341 for (ifp = TAILQ_FIRST(&ifnet); ifp; ifp = TAILQ_NEXT(ifp, if_list)) { 342 if (ifp->if_snd.altq_tbr == NULL) 343 continue; 344 active++; 345 if (!ifq_is_empty(&ifp->if_snd) && ifp->if_start != NULL) { 346 ifnet_serialize_tx(ifp); 347 (*ifp->if_start)(ifp); 348 ifnet_deserialize_tx(ifp); 349 } 350 } 351 crit_exit(); 352 if (active > 0) 353 callout_reset(&tbr_callout, 1, tbr_timeout, NULL); 354 else 355 tbr_timer = 0; /* don't need tbr_timer anymore */ 356 } 357 358 /* 359 * get token bucket regulator profile 360 */ 361 int 362 tbr_get(struct ifaltq *ifq, struct tb_profile *profile) 363 { 364 struct tb_regulator *tbr; 365 366 if ((tbr = ifq->altq_tbr) == NULL) { 367 profile->rate = 0; 368 profile->depth = 0; 369 } else { 370 profile->rate = 371 (u_int)TBR_UNSCALE(tbr->tbr_rate * 8 * machclk_freq); 372 profile->depth = (u_int)TBR_UNSCALE(tbr->tbr_depth); 373 } 374 return (0); 375 } 376 377 /* 378 * attach a discipline to the interface. if one already exists, it is 379 * overridden. 380 */ 381 int 382 altq_pfattach(struct pf_altq *a) 383 { 384 struct ifaltq *ifq; 385 struct ifnet *ifp; 386 int error; 387 388 if (a->scheduler == ALTQT_NONE) 389 return 0; 390 391 if (a->altq_disc == NULL) 392 return EINVAL; 393 394 ifp = ifunit(a->ifname); 395 if (ifp == NULL) 396 return EINVAL; 397 ifq = &ifp->if_snd; 398 399 ALTQ_LOCK(ifq); 400 401 switch (a->scheduler) { 402 #ifdef ALTQ_CBQ 403 case ALTQT_CBQ: 404 error = cbq_pfattach(a, ifq); 405 break; 406 #endif 407 #ifdef ALTQ_PRIQ 408 case ALTQT_PRIQ: 409 error = priq_pfattach(a, ifq); 410 break; 411 #endif 412 #ifdef ALTQ_HFSC 413 case ALTQT_HFSC: 414 error = hfsc_pfattach(a, ifq); 415 break; 416 #endif 417 #ifdef ALTQ_FAIRQ 418 case ALTQT_FAIRQ: 419 error = fairq_pfattach(a, ifq); 420 break; 421 #endif 422 default: 423 error = ENXIO; 424 goto back; 425 } 426 427 /* if the state is running, enable altq */ 428 if (error == 0 && pfaltq_running && ifq->altq_type != ALTQT_NONE && 429 !ifq_is_enabled(ifq)) 430 error = altq_enable_locked(ifq); 431 432 /* if altq is already enabled, reset set tokenbucket regulator */ 433 if (error == 0 && ifq_is_enabled(ifq)) { 434 struct tb_profile tb; 435 436 tb.rate = a->ifbandwidth; 437 tb.depth = a->tbrsize; 438 error = tbr_set_locked(ifq, &tb); 439 } 440 back: 441 ALTQ_UNLOCK(ifq); 442 return (error); 443 } 444 445 /* 446 * detach a discipline from the interface. 447 * it is possible that the discipline was already overridden by another 448 * discipline. 449 */ 450 int 451 altq_pfdetach(struct pf_altq *a) 452 { 453 struct ifnet *ifp; 454 struct ifaltq *ifq; 455 int error = 0; 456 457 ifp = ifunit(a->ifname); 458 if (ifp == NULL) 459 return (EINVAL); 460 ifq = &ifp->if_snd; 461 462 /* if this discipline is no longer referenced, just return */ 463 if (a->altq_disc == NULL) 464 return (0); 465 466 ALTQ_LOCK(ifq); 467 468 if (a->altq_disc != ifq->altq_disc) 469 goto back; 470 471 if (ifq_is_enabled(ifq)) 472 error = altq_disable_locked(ifq); 473 if (error == 0) 474 error = altq_detach_locked(ifq); 475 476 back: 477 ALTQ_UNLOCK(ifq); 478 return (error); 479 } 480 481 /* 482 * add a discipline or a queue 483 */ 484 int 485 altq_add(struct pf_altq *a) 486 { 487 int error = 0; 488 489 if (a->qname[0] != 0) 490 return (altq_add_queue(a)); 491 492 if (machclk_freq == 0) 493 init_machclk(); 494 if (machclk_freq == 0) 495 panic("altq_add: no cpu clock"); 496 497 switch (a->scheduler) { 498 #ifdef ALTQ_CBQ 499 case ALTQT_CBQ: 500 error = cbq_add_altq(a); 501 break; 502 #endif 503 #ifdef ALTQ_PRIQ 504 case ALTQT_PRIQ: 505 error = priq_add_altq(a); 506 break; 507 #endif 508 #ifdef ALTQ_HFSC 509 case ALTQT_HFSC: 510 error = hfsc_add_altq(a); 511 break; 512 #endif 513 #ifdef ALTQ_FAIRQ 514 case ALTQT_FAIRQ: 515 error = fairq_add_altq(a); 516 break; 517 #endif 518 default: 519 error = ENXIO; 520 } 521 522 return (error); 523 } 524 525 /* 526 * remove a discipline or a queue 527 */ 528 int 529 altq_remove(struct pf_altq *a) 530 { 531 int error = 0; 532 533 if (a->qname[0] != 0) 534 return (altq_remove_queue(a)); 535 536 switch (a->scheduler) { 537 #ifdef ALTQ_CBQ 538 case ALTQT_CBQ: 539 error = cbq_remove_altq(a); 540 break; 541 #endif 542 #ifdef ALTQ_PRIQ 543 case ALTQT_PRIQ: 544 error = priq_remove_altq(a); 545 break; 546 #endif 547 #ifdef ALTQ_HFSC 548 case ALTQT_HFSC: 549 error = hfsc_remove_altq(a); 550 break; 551 #endif 552 #ifdef ALTQ_FAIRQ 553 case ALTQT_FAIRQ: 554 error = fairq_remove_altq(a); 555 break; 556 #endif 557 default: 558 error = ENXIO; 559 } 560 561 return (error); 562 } 563 564 /* 565 * add a queue to the discipline 566 */ 567 int 568 altq_add_queue(struct pf_altq *a) 569 { 570 int error = 0; 571 572 switch (a->scheduler) { 573 #ifdef ALTQ_CBQ 574 case ALTQT_CBQ: 575 error = cbq_add_queue(a); 576 break; 577 #endif 578 #ifdef ALTQ_PRIQ 579 case ALTQT_PRIQ: 580 error = priq_add_queue(a); 581 break; 582 #endif 583 #ifdef ALTQ_HFSC 584 case ALTQT_HFSC: 585 error = hfsc_add_queue(a); 586 break; 587 #endif 588 #ifdef ALTQ_FAIRQ 589 case ALTQT_FAIRQ: 590 error = fairq_add_queue(a); 591 break; 592 #endif 593 default: 594 error = ENXIO; 595 } 596 597 return (error); 598 } 599 600 /* 601 * remove a queue from the discipline 602 */ 603 int 604 altq_remove_queue(struct pf_altq *a) 605 { 606 int error = 0; 607 608 switch (a->scheduler) { 609 #ifdef ALTQ_CBQ 610 case ALTQT_CBQ: 611 error = cbq_remove_queue(a); 612 break; 613 #endif 614 #ifdef ALTQ_PRIQ 615 case ALTQT_PRIQ: 616 error = priq_remove_queue(a); 617 break; 618 #endif 619 #ifdef ALTQ_HFSC 620 case ALTQT_HFSC: 621 error = hfsc_remove_queue(a); 622 break; 623 #endif 624 #ifdef ALTQ_FAIRQ 625 case ALTQT_FAIRQ: 626 error = fairq_remove_queue(a); 627 break; 628 #endif 629 default: 630 error = ENXIO; 631 } 632 633 return (error); 634 } 635 636 /* 637 * get queue statistics 638 */ 639 int 640 altq_getqstats(struct pf_altq *a, void *ubuf, int *nbytes) 641 { 642 int error = 0; 643 644 switch (a->scheduler) { 645 #ifdef ALTQ_CBQ 646 case ALTQT_CBQ: 647 error = cbq_getqstats(a, ubuf, nbytes); 648 break; 649 #endif 650 #ifdef ALTQ_PRIQ 651 case ALTQT_PRIQ: 652 error = priq_getqstats(a, ubuf, nbytes); 653 break; 654 #endif 655 #ifdef ALTQ_HFSC 656 case ALTQT_HFSC: 657 error = hfsc_getqstats(a, ubuf, nbytes); 658 break; 659 #endif 660 #ifdef ALTQ_FAIRQ 661 case ALTQT_FAIRQ: 662 error = fairq_getqstats(a, ubuf, nbytes); 663 break; 664 #endif 665 default: 666 error = ENXIO; 667 } 668 669 return (error); 670 } 671 672 /* 673 * read and write diffserv field in IPv4 or IPv6 header 674 */ 675 uint8_t 676 read_dsfield(struct mbuf *m, struct altq_pktattr *pktattr) 677 { 678 struct mbuf *m0; 679 uint8_t ds_field = 0; 680 681 if (pktattr == NULL || 682 (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6)) 683 return ((uint8_t)0); 684 685 /* verify that pattr_hdr is within the mbuf data */ 686 for (m0 = m; m0 != NULL; m0 = m0->m_next) { 687 if ((pktattr->pattr_hdr >= m0->m_data) && 688 (pktattr->pattr_hdr < m0->m_data + m0->m_len)) 689 break; 690 } 691 if (m0 == NULL) { 692 /* ick, pattr_hdr is stale */ 693 pktattr->pattr_af = AF_UNSPEC; 694 #ifdef ALTQ_DEBUG 695 kprintf("read_dsfield: can't locate header!\n"); 696 #endif 697 return ((uint8_t)0); 698 } 699 700 if (pktattr->pattr_af == AF_INET) { 701 struct ip *ip = (struct ip *)pktattr->pattr_hdr; 702 703 if (ip->ip_v != 4) 704 return ((uint8_t)0); /* version mismatch! */ 705 ds_field = ip->ip_tos; 706 } 707 #ifdef INET6 708 else if (pktattr->pattr_af == AF_INET6) { 709 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr; 710 uint32_t flowlabel; 711 712 flowlabel = ntohl(ip6->ip6_flow); 713 if ((flowlabel >> 28) != 6) 714 return ((uint8_t)0); /* version mismatch! */ 715 ds_field = (flowlabel >> 20) & 0xff; 716 } 717 #endif 718 return (ds_field); 719 } 720 721 void 722 write_dsfield(struct mbuf *m, struct altq_pktattr *pktattr, uint8_t dsfield) 723 { 724 struct mbuf *m0; 725 726 if (pktattr == NULL || 727 (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6)) 728 return; 729 730 /* verify that pattr_hdr is within the mbuf data */ 731 for (m0 = m; m0 != NULL; m0 = m0->m_next) { 732 if ((pktattr->pattr_hdr >= m0->m_data) && 733 (pktattr->pattr_hdr < m0->m_data + m0->m_len)) 734 break; 735 } 736 if (m0 == NULL) { 737 /* ick, pattr_hdr is stale */ 738 pktattr->pattr_af = AF_UNSPEC; 739 #ifdef ALTQ_DEBUG 740 kprintf("write_dsfield: can't locate header!\n"); 741 #endif 742 return; 743 } 744 745 if (pktattr->pattr_af == AF_INET) { 746 struct ip *ip = (struct ip *)pktattr->pattr_hdr; 747 uint8_t old; 748 int32_t sum; 749 750 if (ip->ip_v != 4) 751 return; /* version mismatch! */ 752 old = ip->ip_tos; 753 dsfield |= old & 3; /* leave CU bits */ 754 if (old == dsfield) 755 return; 756 ip->ip_tos = dsfield; 757 /* 758 * update checksum (from RFC1624) 759 * HC' = ~(~HC + ~m + m') 760 */ 761 sum = ~ntohs(ip->ip_sum) & 0xffff; 762 sum += 0xff00 + (~old & 0xff) + dsfield; 763 sum = (sum >> 16) + (sum & 0xffff); 764 sum += (sum >> 16); /* add carry */ 765 766 ip->ip_sum = htons(~sum & 0xffff); 767 } 768 #ifdef INET6 769 else if (pktattr->pattr_af == AF_INET6) { 770 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr; 771 uint32_t flowlabel; 772 773 flowlabel = ntohl(ip6->ip6_flow); 774 if ((flowlabel >> 28) != 6) 775 return; /* version mismatch! */ 776 flowlabel = (flowlabel & 0xf03fffff) | (dsfield << 20); 777 ip6->ip6_flow = htonl(flowlabel); 778 } 779 #endif 780 } 781 782 /* 783 * high resolution clock support taking advantage of a machine dependent 784 * high resolution time counter (e.g., timestamp counter of intel pentium). 785 * we assume 786 * - 64-bit-long monotonically-increasing counter 787 * - frequency range is 100M-4GHz (CPU speed) 788 */ 789 /* if pcc is not available or disabled, emulate 256MHz using microtime() */ 790 #define MACHCLK_SHIFT 8 791 792 int machclk_usepcc; 793 uint64_t machclk_freq = 0; 794 uint32_t machclk_per_tick = 0; 795 796 void 797 init_machclk(void) 798 { 799 callout_init(&tbr_callout); 800 801 machclk_usepcc = 1; 802 803 #if !defined(__i386__) || defined(ALTQ_NOPCC) 804 machclk_usepcc = 0; 805 #elif defined(__DragonFly__) && defined(SMP) 806 machclk_usepcc = 0; 807 #elif defined(__i386__) 808 /* check if TSC is available */ 809 if (machclk_usepcc == 1 && (cpu_feature & CPUID_TSC) == 0) 810 machclk_usepcc = 0; 811 #endif 812 813 if (machclk_usepcc == 0) { 814 /* emulate 256MHz using microtime() */ 815 machclk_freq = 1000000LLU << MACHCLK_SHIFT; 816 machclk_per_tick = machclk_freq / hz; 817 #ifdef ALTQ_DEBUG 818 kprintf("altq: emulate %juHz cpu clock\n", (uintmax_t)machclk_freq); 819 #endif 820 return; 821 } 822 823 /* 824 * if the clock frequency (of Pentium TSC or Alpha PCC) is 825 * accessible, just use it. 826 */ 827 #ifdef _RDTSC_SUPPORTED_ 828 if (cpu_feature & CPUID_TSC) 829 machclk_freq = (uint64_t)tsc_frequency; 830 #endif 831 832 /* 833 * if we don't know the clock frequency, measure it. 834 */ 835 if (machclk_freq == 0) { 836 static int wait; 837 struct timeval tv_start, tv_end; 838 uint64_t start, end, diff; 839 int timo; 840 841 microtime(&tv_start); 842 start = read_machclk(); 843 timo = hz; /* 1 sec */ 844 tsleep(&wait, PCATCH, "init_machclk", timo); 845 microtime(&tv_end); 846 end = read_machclk(); 847 diff = (uint64_t)(tv_end.tv_sec - tv_start.tv_sec) * 1000000 848 + tv_end.tv_usec - tv_start.tv_usec; 849 if (diff != 0) 850 machclk_freq = (end - start) * 1000000 / diff; 851 } 852 853 machclk_per_tick = machclk_freq / hz; 854 855 #ifdef ALTQ_DEBUG 856 kprintf("altq: CPU clock: %juHz\n", (uintmax_t)machclk_freq); 857 #endif 858 } 859 860 uint64_t 861 read_machclk(void) 862 { 863 uint64_t val; 864 865 if (machclk_usepcc) { 866 #ifdef _RDTSC_SUPPORTED_ 867 val = rdtsc(); 868 #else 869 panic("read_machclk"); 870 #endif 871 } else { 872 struct timeval tv; 873 874 microtime(&tv); 875 val = (((uint64_t)(tv.tv_sec - boottime.tv_sec) * 1000000 876 + tv.tv_usec) << MACHCLK_SHIFT); 877 } 878 return (val); 879 } 880 881