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