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                     &ifp->if_snd.altq_subq[ALTQ_SUBQ_INDEX_DEFAULT];
 347
 348                 if (ifp->if_snd.altq_tbr == NULL)
 349                         continue;
 350                 active++;
 351                 if (!ifsq_is_empty(ifsq) && ifp->if_start != NULL) {
 352                         ifnet_serialize_tx(ifp);
 353                         (*ifp->if_start)(ifp, ifsq);
 354                         ifnet_deserialize_tx(ifp);
 355                 }
 356         }
 357         crit_exit();
 358         if (active > 0)
 359                 callout_reset(&tbr_callout, 1, tbr_timeout, NULL);
 360         else
 361                 tbr_timer = 0;  /* don't need tbr_timer anymore */
 362 }
 363
 364 /*
 365  * get token bucket regulator profile
 366  */
 367 int
 368 tbr_get(struct ifaltq *ifq, struct tb_profile *profile)
 369 {
 370         struct tb_regulator *tbr;
 371
 372         if ((tbr = ifq->altq_tbr) == NULL) {
 373                 profile->rate = 0;
 374                 profile->depth = 0;
 375         } else {
 376                 profile->rate =
 377                     (u_int)TBR_UNSCALE(tbr->tbr_rate * 8 * machclk_freq);
 378                 profile->depth = (u_int)TBR_UNSCALE(tbr->tbr_depth);
 379         }
 380         return (0);
 381 }
 382
 383 /*
 384  * attach a discipline to the interface.  if one already exists, it is
 385  * overridden.
 386  */
 387 int
 388 altq_pfattach(struct pf_altq *a)
 389 {
 390         struct ifaltq *ifq;
 391         struct ifnet *ifp;
 392         int error;
 393
 394         if (a->scheduler == ALTQT_NONE)
 395                 return 0;
 396
 397         if (a->altq_disc == NULL)
 398                 return EINVAL;
 399
 400         ifp = ifunit(a->ifname);
 401         if (ifp == NULL)
 402                 return EINVAL;
 403         ifq = &ifp->if_snd;
 404
 405         ifq_lock_all(ifq);
 406
 407         switch (a->scheduler) {
 408 #ifdef ALTQ_CBQ
 409         case ALTQT_CBQ:
 410                 error = cbq_pfattach(a, ifq);
 411                 break;
 412 #endif
 413 #ifdef ALTQ_PRIQ
 414         case ALTQT_PRIQ:
 415                 error = priq_pfattach(a, ifq);
 416                 break;
 417 #endif
 418 #ifdef ALTQ_HFSC
 419         case ALTQT_HFSC:
 420                 error = hfsc_pfattach(a, ifq);
 421                 break;
 422 #endif
 423 #ifdef ALTQ_FAIRQ
 424         case ALTQT_FAIRQ:
 425                 error = fairq_pfattach(a, ifq);
 426                 break;
 427 #endif
 428         default:
 429                 error = ENXIO;
 430                 goto back;
 431         }
 432
 433         /* if the state is running, enable altq */
 434         if (error == 0 && pfaltq_running && ifq->altq_type != ALTQT_NONE &&
 435             !ifq_is_enabled(ifq))
 436                 error = altq_enable_locked(ifq);
 437
 438         /* if altq is already enabled, reset set tokenbucket regulator */
 439         if (error == 0 && ifq_is_enabled(ifq)) {
 440                 struct tb_profile tb;
 441
 442                 tb.rate = a->ifbandwidth;
 443                 tb.depth = a->tbrsize;
 444                 error = tbr_set_locked(ifq, &tb);
 445         }
 446 back:
 447         ifq_unlock_all(ifq);
 448         return (error);
 449 }
 450
 451 /*
 452  * detach a discipline from the interface.
 453  * it is possible that the discipline was already overridden by another
 454  * discipline.
 455  */
 456 int
 457 altq_pfdetach(struct pf_altq *a)
 458 {
 459         struct ifnet *ifp;
 460         struct ifaltq *ifq;
 461         int error = 0;
 462
 463         ifp = ifunit(a->ifname);
 464         if (ifp == NULL)
 465                 return (EINVAL);
 466         ifq = &ifp->if_snd;
 467
 468         /* if this discipline is no longer referenced, just return */
 469         if (a->altq_disc == NULL)
 470                 return (0);
 471
 472         ifq_lock_all(ifq);
 473
 474         if (a->altq_disc != ifq->altq_disc)
 475                 goto back;
 476
 477         if (ifq_is_enabled(ifq))
 478                 error = altq_disable_locked(ifq);
 479         if (error == 0)
 480                 error = altq_detach_locked(ifq);
 481
 482 back:
 483         ifq_unlock_all(ifq);
 484         return (error);
 485 }
 486
 487 /*
 488  * add a discipline or a queue
 489  */
 490 int
 491 altq_add(struct pf_altq *a)
 492 {
 493         int error = 0;
 494
 495         if (a->qname[0] != 0)
 496                 return (altq_add_queue(a));
 497
 498         if (machclk_freq == 0)
 499                 init_machclk();
 500         if (machclk_freq == 0)
 501                 panic("altq_add: no cpu clock");
 502
 503         switch (a->scheduler) {
 504 #ifdef ALTQ_CBQ
 505         case ALTQT_CBQ:
 506                 error = cbq_add_altq(a);
 507                 break;
 508 #endif
 509 #ifdef ALTQ_PRIQ
 510         case ALTQT_PRIQ:
 511                 error = priq_add_altq(a);
 512                 break;
 513 #endif
 514 #ifdef ALTQ_HFSC
 515         case ALTQT_HFSC:
 516                 error = hfsc_add_altq(a);
 517                 break;
 518 #endif
 519 #ifdef ALTQ_FAIRQ
 520         case ALTQT_FAIRQ:
 521                 error = fairq_add_altq(a);
 522                 break;
 523 #endif
 524         default:
 525                 error = ENXIO;
 526         }
 527
 528         return (error);
 529 }
 530
 531 /*
 532  * remove a discipline or a queue
 533  */
 534 int
 535 altq_remove(struct pf_altq *a)
 536 {
 537         int error = 0;
 538
 539         if (a->qname[0] != 0)
 540                 return (altq_remove_queue(a));
 541
 542         switch (a->scheduler) {
 543 #ifdef ALTQ_CBQ
 544         case ALTQT_CBQ:
 545                 error = cbq_remove_altq(a);
 546                 break;
 547 #endif
 548 #ifdef ALTQ_PRIQ
 549         case ALTQT_PRIQ:
 550                 error = priq_remove_altq(a);
 551                 break;
 552 #endif
 553 #ifdef ALTQ_HFSC
 554         case ALTQT_HFSC:
 555                 error = hfsc_remove_altq(a);
 556                 break;
 557 #endif
 558 #ifdef ALTQ_FAIRQ
 559         case ALTQT_FAIRQ:
 560                 error = fairq_remove_altq(a);
 561                 break;
 562 #endif
 563         default:
 564                 error = ENXIO;
 565         }
 566
 567         return (error);
 568 }
 569
 570 /*
 571  * add a queue to the discipline
 572  */
 573 int
 574 altq_add_queue(struct pf_altq *a)
 575 {
 576         int error = 0;
 577
 578         switch (a->scheduler) {
 579 #ifdef ALTQ_CBQ
 580         case ALTQT_CBQ:
 581                 error = cbq_add_queue(a);
 582                 break;
 583 #endif
 584 #ifdef ALTQ_PRIQ
 585         case ALTQT_PRIQ:
 586                 error = priq_add_queue(a);
 587                 break;
 588 #endif
 589 #ifdef ALTQ_HFSC
 590         case ALTQT_HFSC:
 591                 error = hfsc_add_queue(a);
 592                 break;
 593 #endif
 594 #ifdef ALTQ_FAIRQ
 595         case ALTQT_FAIRQ:
 596                 error = fairq_add_queue(a);
 597                 break;
 598 #endif
 599         default:
 600                 error = ENXIO;
 601         }
 602
 603         return (error);
 604 }
 605
 606 /*
 607  * remove a queue from the discipline
 608  */
 609 int
 610 altq_remove_queue(struct pf_altq *a)
 611 {
 612         int error = 0;
 613
 614         switch (a->scheduler) {
 615 #ifdef ALTQ_CBQ
 616         case ALTQT_CBQ:
 617                 error = cbq_remove_queue(a);
 618                 break;
 619 #endif
 620 #ifdef ALTQ_PRIQ
 621         case ALTQT_PRIQ:
 622                 error = priq_remove_queue(a);
 623                 break;
 624 #endif
 625 #ifdef ALTQ_HFSC
 626         case ALTQT_HFSC:
 627                 error = hfsc_remove_queue(a);
 628                 break;
 629 #endif
 630 #ifdef ALTQ_FAIRQ
 631         case ALTQT_FAIRQ:
 632                 error = fairq_remove_queue(a);
 633                 break;
 634 #endif
 635         default:
 636                 error = ENXIO;
 637         }
 638
 639         return (error);
 640 }
 641
 642 /*
 643  * get queue statistics
 644  */
 645 int
 646 altq_getqstats(struct pf_altq *a, void *ubuf, int *nbytes)
 647 {
 648         int error = 0;
 649
 650         switch (a->scheduler) {
 651 #ifdef ALTQ_CBQ
 652         case ALTQT_CBQ:
 653                 error = cbq_getqstats(a, ubuf, nbytes);
 654                 break;
 655 #endif
 656 #ifdef ALTQ_PRIQ
 657         case ALTQT_PRIQ:
 658                 error = priq_getqstats(a, ubuf, nbytes);
 659                 break;
 660 #endif
 661 #ifdef ALTQ_HFSC
 662         case ALTQT_HFSC:
 663                 error = hfsc_getqstats(a, ubuf, nbytes);
 664                 break;
 665 #endif
 666 #ifdef ALTQ_FAIRQ
 667         case ALTQT_FAIRQ:
 668                 error = fairq_getqstats(a, ubuf, nbytes);
 669                 break;
 670 #endif
 671         default:
 672                 error = ENXIO;
 673         }
 674
 675         return (error);
 676 }
 677
 678 /*
 679  * read and write diffserv field in IPv4 or IPv6 header
 680  */
 681 uint8_t
 682 read_dsfield(struct mbuf *m, struct altq_pktattr *pktattr)
 683 {
 684         struct mbuf *m0;
 685         uint8_t ds_field = 0;
 686
 687         if (pktattr == NULL ||
 688             (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6))
 689                 return ((uint8_t)0);
 690
 691         /* verify that pattr_hdr is within the mbuf data */
 692         for (m0 = m; m0 != NULL; m0 = m0->m_next) {
 693                 if ((pktattr->pattr_hdr >= m0->m_data) &&
 694                     (pktattr->pattr_hdr < m0->m_data + m0->m_len))
 695                         break;
 696         }
 697         if (m0 == NULL) {
 698                 /* ick, pattr_hdr is stale */
 699                 pktattr->pattr_af = AF_UNSPEC;
 700 #ifdef ALTQ_DEBUG
 701                 kprintf("read_dsfield: can't locate header!\n");
 702 #endif
 703                 return ((uint8_t)0);
 704         }
 705
 706         if (pktattr->pattr_af == AF_INET) {
 707                 struct ip *ip = (struct ip *)pktattr->pattr_hdr;
 708
 709                 if (ip->ip_v != 4)
 710                         return ((uint8_t)0);    /* version mismatch! */
 711                 ds_field = ip->ip_tos;
 712         }
 713 #ifdef INET6
 714         else if (pktattr->pattr_af == AF_INET6) {
 715                 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr;
 716                 uint32_t flowlabel;
 717
 718                 flowlabel = ntohl(ip6->ip6_flow);
 719                 if ((flowlabel >> 28) != 6)
 720                         return ((uint8_t)0);    /* version mismatch! */
 721                 ds_field = (flowlabel >> 20) & 0xff;
 722         }
 723 #endif
 724         return (ds_field);
 725 }
 726
 727 void
 728 write_dsfield(struct mbuf *m, struct altq_pktattr *pktattr, uint8_t dsfield)
 729 {
 730         struct mbuf *m0;
 731
 732         if (pktattr == NULL ||
 733             (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6))
 734                 return;
 735
 736         /* verify that pattr_hdr is within the mbuf data */
 737         for (m0 = m; m0 != NULL; m0 = m0->m_next) {
 738                 if ((pktattr->pattr_hdr >= m0->m_data) &&
 739                     (pktattr->pattr_hdr < m0->m_data + m0->m_len))
 740                         break;
 741         }
 742         if (m0 == NULL) {
 743                 /* ick, pattr_hdr is stale */
 744                 pktattr->pattr_af = AF_UNSPEC;
 745 #ifdef ALTQ_DEBUG
 746                 kprintf("write_dsfield: can't locate header!\n");
 747 #endif
 748                 return;
 749         }
 750
 751         if (pktattr->pattr_af == AF_INET) {
 752                 struct ip *ip = (struct ip *)pktattr->pattr_hdr;
 753                 uint8_t old;
 754                 int32_t sum;
 755
 756                 if (ip->ip_v != 4)
 757                         return;         /* version mismatch! */
 758                 old = ip->ip_tos;
 759                 dsfield |= old & 3;     /* leave CU bits */
 760                 if (old == dsfield)
 761                         return;
 762                 ip->ip_tos = dsfield;
 763                 /*
 764                  * update checksum (from RFC1624)
 765                  *         HC' = ~(~HC + ~m + m')
 766                  */
 767                 sum = ~ntohs(ip->ip_sum) & 0xffff;
 768                 sum += 0xff00 + (~old & 0xff) + dsfield;
 769                 sum = (sum >> 16) + (sum & 0xffff);
 770                 sum += (sum >> 16);  /* add carry */
 771
 772                 ip->ip_sum = htons(~sum & 0xffff);
 773         }
 774 #ifdef INET6
 775         else if (pktattr->pattr_af == AF_INET6) {
 776                 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr;
 777                 uint32_t flowlabel;
 778
 779                 flowlabel = ntohl(ip6->ip6_flow);
 780                 if ((flowlabel >> 28) != 6)
 781                         return;         /* version mismatch! */
 782                 flowlabel = (flowlabel & 0xf03fffff) | (dsfield << 20);
 783                 ip6->ip6_flow = htonl(flowlabel);
 784         }
 785 #endif
 786 }
 787
 788 /*
 789  * high resolution clock support taking advantage of a machine dependent
 790  * high resolution time counter (e.g., timestamp counter of intel pentium).
 791  * we assume
 792  *  - 64-bit-long monotonically-increasing counter
 793  *  - frequency range is 100M-4GHz (CPU speed)
 794  */
 795 /* if pcc is not available or disabled, emulate 256MHz using microtime() */
 796 #define MACHCLK_SHIFT   8
 797
 798 int machclk_usepcc;
 799 uint64_t machclk_freq = 0;
 800 uint32_t machclk_per_tick = 0;
 801
 802 void
 803 init_machclk(void)
 804 {
 805         callout_init(&tbr_callout);
 806
 807         machclk_usepcc = 1;
 808
 809 #if !defined(__i386__) || defined(ALTQ_NOPCC)
 810         machclk_usepcc = 0;
 811 #elif defined(__DragonFly__)
 812         machclk_usepcc = 0;
 813 #elif defined(__i386__)
 814         /* check if TSC is available */
 815         if (machclk_usepcc == 1 && (cpu_feature & CPUID_TSC) == 0)
 816                 machclk_usepcc = 0;
 817 #endif
 818
 819         if (machclk_usepcc == 0) {
 820                 /* emulate 256MHz using microtime() */
 821                 machclk_freq = 1000000LLU << MACHCLK_SHIFT;
 822                 machclk_per_tick = machclk_freq / hz;
 823 #ifdef ALTQ_DEBUG
 824                 kprintf("altq: emulate %juHz cpu clock\n", (uintmax_t)machclk_freq);
 825 #endif
 826                 return;
 827         }
 828
 829         /*
 830          * if the clock frequency (of Pentium TSC or Alpha PCC) is
 831          * accessible, just use it.
 832          */
 833 #ifdef _RDTSC_SUPPORTED_
 834         if (cpu_feature & CPUID_TSC)
 835                 machclk_freq = (uint64_t)tsc_frequency;
 836 #endif
 837
 838         /*
 839          * if we don't know the clock frequency, measure it.
 840          */
 841         if (machclk_freq == 0) {
 842                 static int      wait;
 843                 struct timeval  tv_start, tv_end;
 844                 uint64_t        start, end, diff;
 845                 int             timo;
 846
 847                 microtime(&tv_start);
 848                 start = read_machclk();
 849                 timo = hz;      /* 1 sec */
 850                 tsleep(&wait, PCATCH, "init_machclk", timo);
 851                 microtime(&tv_end);
 852                 end = read_machclk();
 853                 diff = (uint64_t)(tv_end.tv_sec - tv_start.tv_sec) * 1000000
 854                     + tv_end.tv_usec - tv_start.tv_usec;
 855                 if (diff != 0)
 856                         machclk_freq = (end - start) * 1000000 / diff;
 857         }
 858
 859         machclk_per_tick = machclk_freq / hz;
 860
 861 #ifdef ALTQ_DEBUG
 862         kprintf("altq: CPU clock: %juHz\n", (uintmax_t)machclk_freq);
 863 #endif
 864 }
 865
 866 uint64_t
 867 read_machclk(void)
 868 {
 869         uint64_t val;
 870
 871         if (machclk_usepcc) {
 872 #ifdef _RDTSC_SUPPORTED_
 873                 val = rdtsc();
 874 #else
 875                 panic("read_machclk");
 876 #endif
 877         } else {
 878                 struct timeval tv;
 879
 880                 microtime(&tv);
 881                 val = (((uint64_t)(tv.tv_sec - boottime.tv_sec) * 1000000
 882                     + tv.tv_usec) << MACHCLK_SHIFT);
 883         }
 884         return (val);
 885 }
 886