sys/net/altq/altq_subr.c

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