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