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