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