sys/contrib/altq/altq/altq_subr.c

   1 /*      $FreeBSD$       */
   2 /*      $KAME: altq_subr.c,v 1.21 2003/11/06 06:32:53 kjc 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 #if defined(__FreeBSD__) || defined(__NetBSD__)
  31 #include "opt_altq.h"
  32 #include "opt_inet.h"
  33 #ifdef __FreeBSD__
  34 #include "opt_inet6.h"
  35 #endif
  36 #endif /* __FreeBSD__ || __NetBSD__ */
  37
  38 #include <sys/param.h>
  39 #include <sys/malloc.h>
  40 #include <sys/mbuf.h>
  41 #include <sys/systm.h>
  42 #include <sys/proc.h>
  43 #include <sys/socket.h>
  44 #include <sys/socketvar.h>
  45 #include <sys/kernel.h>
  46 #include <sys/errno.h>
  47 #include <sys/syslog.h>
  48 #include <sys/sysctl.h>
  49 #include <sys/queue.h>
  50
  51 #include <net/if.h>
  52 #include <net/if_dl.h>
  53 #include <net/if_types.h>
  54 #ifdef __FreeBSD__
  55 #include <net/vnet.h>
  56 #endif
  57
  58 #include <netinet/in.h>
  59 #include <netinet/in_systm.h>
  60 #include <netinet/ip.h>
  61 #ifdef INET6
  62 #include <netinet/ip6.h>
  63 #endif
  64 #include <netinet/tcp.h>
  65 #include <netinet/udp.h>
  66
  67 #include <net/pfvar.h>
  68 #include <altq/altq.h>
  69 #ifdef ALTQ3_COMPAT
  70 #include <altq/altq_conf.h>
  71 #endif
  72
  73 /* machine dependent clock related includes */
  74 #ifdef __FreeBSD__
  75 #include <sys/bus.h>
  76 #include <sys/cpu.h>
  77 #include <sys/eventhandler.h>
  78 #include <machine/clock.h>
  79 #endif
  80 #if defined(__amd64__) || defined(__i386__)
  81 #include <machine/cpufunc.h>            /* for pentium tsc */
  82 #include <machine/specialreg.h>         /* for CPUID_TSC */
  83 #ifdef __FreeBSD__
  84 #include <machine/md_var.h>             /* for cpu_feature */
  85 #elif defined(__NetBSD__) || defined(__OpenBSD__)
  86 #include <machine/cpu.h>                /* for cpu_feature */
  87 #endif
  88 #endif /* __amd64 || __i386__ */
  89
  90 /*
  91  * internal function prototypes
  92  */
  93 static void     tbr_timeout(void *);
  94 int (*altq_input)(struct mbuf *, int) = NULL;
  95 static struct mbuf *tbr_dequeue(struct ifaltq *, int);
  96 static int tbr_timer = 0;       /* token bucket regulator timer */
  97 #if !defined(__FreeBSD__) || (__FreeBSD_version < 600000)
  98 static struct callout tbr_callout = CALLOUT_INITIALIZER;
  99 #else
 100 static struct callout tbr_callout;
 101 #endif
 102
 103 #ifdef ALTQ3_CLFIER_COMPAT
 104 static int      extract_ports4(struct mbuf *, struct ip *, struct flowinfo_in *);
 105 #ifdef INET6
 106 static int      extract_ports6(struct mbuf *, struct ip6_hdr *,
 107                                struct flowinfo_in6 *);
 108 #endif
 109 static int      apply_filter4(u_int32_t, struct flow_filter *,
 110                               struct flowinfo_in *);
 111 static int      apply_ppfilter4(u_int32_t, struct flow_filter *,
 112                                 struct flowinfo_in *);
 113 #ifdef INET6
 114 static int      apply_filter6(u_int32_t, struct flow_filter6 *,
 115                               struct flowinfo_in6 *);
 116 #endif
 117 static int      apply_tosfilter4(u_int32_t, struct flow_filter *,
 118                                  struct flowinfo_in *);
 119 static u_long   get_filt_handle(struct acc_classifier *, int);
 120 static struct acc_filter *filth_to_filtp(struct acc_classifier *, u_long);
 121 static u_int32_t filt2fibmask(struct flow_filter *);
 122
 123 static void     ip4f_cache(struct ip *, struct flowinfo_in *);
 124 static int      ip4f_lookup(struct ip *, struct flowinfo_in *);
 125 static int      ip4f_init(void);
 126 static struct ip4_frag  *ip4f_alloc(void);
 127 static void     ip4f_free(struct ip4_frag *);
 128 #endif /* ALTQ3_CLFIER_COMPAT */
 129
 130 /*
 131  * alternate queueing support routines
 132  */
 133
 134 /* look up the queue state by the interface name and the queueing type. */
 135 void *
 136 altq_lookup(name, type)
 137         char *name;
 138         int type;
 139 {
 140         struct ifnet *ifp;
 141
 142         if ((ifp = ifunit(name)) != NULL) {
 143                 /* read if_snd unlocked */
 144                 if (type != ALTQT_NONE && ifp->if_snd.altq_type == type)
 145                         return (ifp->if_snd.altq_disc);
 146         }
 147
 148         return NULL;
 149 }
 150
 151 int
 152 altq_attach(ifq, type, discipline, enqueue, dequeue, request, clfier, classify)
 153         struct ifaltq *ifq;
 154         int type;
 155         void *discipline;
 156         int (*enqueue)(struct ifaltq *, struct mbuf *, struct altq_pktattr *);
 157         struct mbuf *(*dequeue)(struct ifaltq *, int);
 158         int (*request)(struct ifaltq *, int, void *);
 159         void *clfier;
 160         void *(*classify)(void *, struct mbuf *, int);
 161 {
 162         IFQ_LOCK(ifq);
 163         if (!ALTQ_IS_READY(ifq)) {
 164                 IFQ_UNLOCK(ifq);
 165                 return ENXIO;
 166         }
 167
 168 #ifdef ALTQ3_COMPAT
 169         /*
 170          * pfaltq can override the existing discipline, but altq3 cannot.
 171          * check these if clfier is not NULL (which implies altq3).
 172          */
 173         if (clfier != NULL) {
 174                 if (ALTQ_IS_ENABLED(ifq)) {
 175                         IFQ_UNLOCK(ifq);
 176                         return EBUSY;
 177                 }
 178                 if (ALTQ_IS_ATTACHED(ifq)) {
 179                         IFQ_UNLOCK(ifq);
 180                         return EEXIST;
 181                 }
 182         }
 183 #endif
 184         ifq->altq_type     = type;
 185         ifq->altq_disc     = discipline;
 186         ifq->altq_enqueue  = enqueue;
 187         ifq->altq_dequeue  = dequeue;
 188         ifq->altq_request  = request;
 189         ifq->altq_clfier   = clfier;
 190         ifq->altq_classify = classify;
 191         ifq->altq_flags &= (ALTQF_CANTCHANGE|ALTQF_ENABLED);
 192 #ifdef ALTQ3_COMPAT
 193 #ifdef ALTQ_KLD
 194         altq_module_incref(type);
 195 #endif
 196 #endif
 197         IFQ_UNLOCK(ifq);
 198         return 0;
 199 }
 200
 201 int
 202 altq_detach(ifq)
 203         struct ifaltq *ifq;
 204 {
 205         IFQ_LOCK(ifq);
 206
 207         if (!ALTQ_IS_READY(ifq)) {
 208                 IFQ_UNLOCK(ifq);
 209                 return ENXIO;
 210         }
 211         if (ALTQ_IS_ENABLED(ifq)) {
 212                 IFQ_UNLOCK(ifq);
 213                 return EBUSY;
 214         }
 215         if (!ALTQ_IS_ATTACHED(ifq)) {
 216                 IFQ_UNLOCK(ifq);
 217                 return (0);
 218         }
 219 #ifdef ALTQ3_COMPAT
 220 #ifdef ALTQ_KLD
 221         altq_module_declref(ifq->altq_type);
 222 #endif
 223 #endif
 224
 225         ifq->altq_type     = ALTQT_NONE;
 226         ifq->altq_disc     = NULL;
 227         ifq->altq_enqueue  = NULL;
 228         ifq->altq_dequeue  = NULL;
 229         ifq->altq_request  = NULL;
 230         ifq->altq_clfier   = NULL;
 231         ifq->altq_classify = NULL;
 232         ifq->altq_flags &= ALTQF_CANTCHANGE;
 233
 234         IFQ_UNLOCK(ifq);
 235         return 0;
 236 }
 237
 238 int
 239 altq_enable(ifq)
 240         struct ifaltq *ifq;
 241 {
 242         int s;
 243
 244         IFQ_LOCK(ifq);
 245
 246         if (!ALTQ_IS_READY(ifq)) {
 247                 IFQ_UNLOCK(ifq);
 248                 return ENXIO;
 249         }
 250         if (ALTQ_IS_ENABLED(ifq)) {
 251                 IFQ_UNLOCK(ifq);
 252                 return 0;
 253         }
 254
 255 #ifdef __NetBSD__
 256         s = splnet();
 257 #else
 258         s = splimp();
 259 #endif
 260         IFQ_PURGE_NOLOCK(ifq);
 261         ASSERT(ifq->ifq_len == 0);
 262         ifq->ifq_drv_maxlen = 0;                /* disable bulk dequeue */
 263         ifq->altq_flags |= ALTQF_ENABLED;
 264         if (ifq->altq_clfier != NULL)
 265                 ifq->altq_flags |= ALTQF_CLASSIFY;
 266         splx(s);
 267
 268         IFQ_UNLOCK(ifq);
 269         return 0;
 270 }
 271
 272 int
 273 altq_disable(ifq)
 274         struct ifaltq *ifq;
 275 {
 276         int s;
 277
 278         IFQ_LOCK(ifq);
 279         if (!ALTQ_IS_ENABLED(ifq)) {
 280                 IFQ_UNLOCK(ifq);
 281                 return 0;
 282         }
 283
 284 #ifdef __NetBSD__
 285         s = splnet();
 286 #else
 287         s = splimp();
 288 #endif
 289         IFQ_PURGE_NOLOCK(ifq);
 290         ASSERT(ifq->ifq_len == 0);
 291         ifq->altq_flags &= ~(ALTQF_ENABLED|ALTQF_CLASSIFY);
 292         splx(s);
 293
 294         IFQ_UNLOCK(ifq);
 295         return 0;
 296 }
 297
 298 #ifdef ALTQ_DEBUG
 299 void
 300 altq_assert(file, line, failedexpr)
 301         const char *file, *failedexpr;
 302         int line;
 303 {
 304         (void)printf("altq assertion \"%s\" failed: file \"%s\", line %d\n",
 305                      failedexpr, file, line);
 306         panic("altq assertion");
 307         /* NOTREACHED */
 308 }
 309 #endif
 310
 311 /*
 312  * internal representation of token bucket parameters
 313  *      rate:   byte_per_unittime << 32
 314  *              (((bits_per_sec) / 8) << 32) / machclk_freq
 315  *      depth:  byte << 32
 316  *
 317  */
 318 #define TBR_SHIFT       32
 319 #define TBR_SCALE(x)    ((int64_t)(x) << TBR_SHIFT)
 320 #define TBR_UNSCALE(x)  ((x) >> TBR_SHIFT)
 321
 322 static struct mbuf *
 323 tbr_dequeue(ifq, op)
 324         struct ifaltq *ifq;
 325         int op;
 326 {
 327         struct tb_regulator *tbr;
 328         struct mbuf *m;
 329         int64_t interval;
 330         u_int64_t now;
 331
 332         IFQ_LOCK_ASSERT(ifq);
 333         tbr = ifq->altq_tbr;
 334         if (op == ALTDQ_REMOVE && tbr->tbr_lastop == ALTDQ_POLL) {
 335                 /* if this is a remove after poll, bypass tbr check */
 336         } else {
 337                 /* update token only when it is negative */
 338                 if (tbr->tbr_token <= 0) {
 339                         now = read_machclk();
 340                         interval = now - tbr->tbr_last;
 341                         if (interval >= tbr->tbr_filluptime)
 342                                 tbr->tbr_token = tbr->tbr_depth;
 343                         else {
 344                                 tbr->tbr_token += interval * tbr->tbr_rate;
 345                                 if (tbr->tbr_token > tbr->tbr_depth)
 346                                         tbr->tbr_token = tbr->tbr_depth;
 347                         }
 348                         tbr->tbr_last = now;
 349                 }
 350                 /* if token is still negative, don't allow dequeue */
 351                 if (tbr->tbr_token <= 0)
 352                         return (NULL);
 353         }
 354
 355         if (ALTQ_IS_ENABLED(ifq))
 356                 m = (*ifq->altq_dequeue)(ifq, op);
 357         else {
 358                 if (op == ALTDQ_POLL)
 359                         _IF_POLL(ifq, m);
 360                 else
 361                         _IF_DEQUEUE(ifq, m);
 362         }
 363
 364         if (m != NULL && op == ALTDQ_REMOVE)
 365                 tbr->tbr_token -= TBR_SCALE(m_pktlen(m));
 366         tbr->tbr_lastop = op;
 367         return (m);
 368 }
 369
 370 /*
 371  * set a token bucket regulator.
 372  * if the specified rate is zero, the token bucket regulator is deleted.
 373  */
 374 int
 375 tbr_set(ifq, profile)
 376         struct ifaltq *ifq;
 377         struct tb_profile *profile;
 378 {
 379         struct tb_regulator *tbr, *otbr;
 380
 381         if (tbr_dequeue_ptr == NULL)
 382                 tbr_dequeue_ptr = tbr_dequeue;
 383
 384         if (machclk_freq == 0)
 385                 init_machclk();
 386         if (machclk_freq == 0) {
 387                 printf("tbr_set: no cpu clock available!\n");
 388                 return (ENXIO);
 389         }
 390
 391         IFQ_LOCK(ifq);
 392         if (profile->rate == 0) {
 393                 /* delete this tbr */
 394                 if ((tbr = ifq->altq_tbr) == NULL) {
 395                         IFQ_UNLOCK(ifq);
 396                         return (ENOENT);
 397                 }
 398                 ifq->altq_tbr = NULL;
 399                 free(tbr, M_DEVBUF);
 400                 IFQ_UNLOCK(ifq);
 401                 return (0);
 402         }
 403
 404         tbr = malloc(sizeof(struct tb_regulator), M_DEVBUF, M_NOWAIT | M_ZERO);
 405         if (tbr == NULL) {
 406                 IFQ_UNLOCK(ifq);
 407                 return (ENOMEM);
 408         }
 409
 410         tbr->tbr_rate = TBR_SCALE(profile->rate / 8) / machclk_freq;
 411         tbr->tbr_depth = TBR_SCALE(profile->depth);
 412         if (tbr->tbr_rate > 0)
 413                 tbr->tbr_filluptime = tbr->tbr_depth / tbr->tbr_rate;
 414         else
 415                 tbr->tbr_filluptime = 0xffffffffffffffffLL;
 416         tbr->tbr_token = tbr->tbr_depth;
 417         tbr->tbr_last = read_machclk();
 418         tbr->tbr_lastop = ALTDQ_REMOVE;
 419
 420         otbr = ifq->altq_tbr;
 421         ifq->altq_tbr = tbr;    /* set the new tbr */
 422
 423         if (otbr != NULL)
 424                 free(otbr, M_DEVBUF);
 425         else {
 426                 if (tbr_timer == 0) {
 427                         CALLOUT_RESET(&tbr_callout, 1, tbr_timeout, (void *)0);
 428                         tbr_timer = 1;
 429                 }
 430         }
 431         IFQ_UNLOCK(ifq);
 432         return (0);
 433 }
 434
 435 /*
 436  * tbr_timeout goes through the interface list, and kicks the drivers
 437  * if necessary.
 438  *
 439  * MPSAFE
 440  */
 441 static void
 442 tbr_timeout(arg)
 443         void *arg;
 444 {
 445 #ifdef __FreeBSD__
 446         VNET_ITERATOR_DECL(vnet_iter);
 447 #endif
 448         struct ifnet *ifp;
 449         int active, s;
 450
 451         active = 0;
 452 #ifdef __NetBSD__
 453         s = splnet();
 454 #else
 455         s = splimp();
 456 #endif
 457 #ifdef __FreeBSD__
 458         IFNET_RLOCK_NOSLEEP();
 459         VNET_LIST_RLOCK_NOSLEEP();
 460         VNET_FOREACH(vnet_iter) {
 461                 CURVNET_SET(vnet_iter);
 462 #endif
 463                 for (ifp = TAILQ_FIRST(&V_ifnet); ifp;
 464                     ifp = TAILQ_NEXT(ifp, if_list)) {
 465                         /* read from if_snd unlocked */
 466                         if (!TBR_IS_ENABLED(&ifp->if_snd))
 467                                 continue;
 468                         active++;
 469                         if (!IFQ_IS_EMPTY(&ifp->if_snd) &&
 470                             ifp->if_start != NULL)
 471                                 (*ifp->if_start)(ifp);
 472                 }
 473 #ifdef __FreeBSD__
 474                 CURVNET_RESTORE();
 475         }
 476         VNET_LIST_RUNLOCK_NOSLEEP();
 477         IFNET_RUNLOCK_NOSLEEP();
 478 #endif
 479         splx(s);
 480         if (active > 0)
 481                 CALLOUT_RESET(&tbr_callout, 1, tbr_timeout, (void *)0);
 482         else
 483                 tbr_timer = 0;  /* don't need tbr_timer anymore */
 484 }
 485
 486 /*
 487  * get token bucket regulator profile
 488  */
 489 int
 490 tbr_get(ifq, profile)
 491         struct ifaltq *ifq;
 492         struct tb_profile *profile;
 493 {
 494         struct tb_regulator *tbr;
 495
 496         IFQ_LOCK(ifq);
 497         if ((tbr = ifq->altq_tbr) == NULL) {
 498                 profile->rate = 0;
 499                 profile->depth = 0;
 500         } else {
 501                 profile->rate =
 502                     (u_int)TBR_UNSCALE(tbr->tbr_rate * 8 * machclk_freq);
 503                 profile->depth = (u_int)TBR_UNSCALE(tbr->tbr_depth);
 504         }
 505         IFQ_UNLOCK(ifq);
 506         return (0);
 507 }
 508
 509 /*
 510  * attach a discipline to the interface.  if one already exists, it is
 511  * overridden.
 512  * Locking is done in the discipline specific attach functions. Basically
 513  * they call back to altq_attach which takes care of the attach and locking.
 514  */
 515 int
 516 altq_pfattach(struct pf_altq *a)
 517 {
 518         int error = 0;
 519
 520         switch (a->scheduler) {
 521         case ALTQT_NONE:
 522                 break;
 523 #ifdef ALTQ_CBQ
 524         case ALTQT_CBQ:
 525                 error = cbq_pfattach(a);
 526                 break;
 527 #endif
 528 #ifdef ALTQ_PRIQ
 529         case ALTQT_PRIQ:
 530                 error = priq_pfattach(a);
 531                 break;
 532 #endif
 533 #ifdef ALTQ_HFSC
 534         case ALTQT_HFSC:
 535                 error = hfsc_pfattach(a);
 536                 break;
 537 #endif
 538         default:
 539                 error = ENXIO;
 540         }
 541
 542         return (error);
 543 }
 544
 545 /*
 546  * detach a discipline from the interface.
 547  * it is possible that the discipline was already overridden by another
 548  * discipline.
 549  */
 550 int
 551 altq_pfdetach(struct pf_altq *a)
 552 {
 553         struct ifnet *ifp;
 554         int s, error = 0;
 555
 556         if ((ifp = ifunit(a->ifname)) == NULL)
 557                 return (EINVAL);
 558
 559         /* if this discipline is no longer referenced, just return */
 560         /* read unlocked from if_snd */
 561         if (a->altq_disc == NULL || a->altq_disc != ifp->if_snd.altq_disc)
 562                 return (0);
 563
 564 #ifdef __NetBSD__
 565         s = splnet();
 566 #else
 567         s = splimp();
 568 #endif
 569         /* read unlocked from if_snd, _disable and _detach take care */
 570         if (ALTQ_IS_ENABLED(&ifp->if_snd))
 571                 error = altq_disable(&ifp->if_snd);
 572         if (error == 0)
 573                 error = altq_detach(&ifp->if_snd);
 574         splx(s);
 575
 576         return (error);
 577 }
 578
 579 /*
 580  * add a discipline or a queue
 581  * Locking is done in the discipline specific functions with regards to
 582  * malloc with WAITOK, also it is not yet clear which lock to use.
 583  */
 584 int
 585 altq_add(struct pf_altq *a)
 586 {
 587         int error = 0;
 588
 589         if (a->qname[0] != 0)
 590                 return (altq_add_queue(a));
 591
 592         if (machclk_freq == 0)
 593                 init_machclk();
 594         if (machclk_freq == 0)
 595                 panic("altq_add: no cpu clock");
 596
 597         switch (a->scheduler) {
 598 #ifdef ALTQ_CBQ
 599         case ALTQT_CBQ:
 600                 error = cbq_add_altq(a);
 601                 break;
 602 #endif
 603 #ifdef ALTQ_PRIQ
 604         case ALTQT_PRIQ:
 605                 error = priq_add_altq(a);
 606                 break;
 607 #endif
 608 #ifdef ALTQ_HFSC
 609         case ALTQT_HFSC:
 610                 error = hfsc_add_altq(a);
 611                 break;
 612 #endif
 613         default:
 614                 error = ENXIO;
 615         }
 616
 617         return (error);
 618 }
 619
 620 /*
 621  * remove a discipline or a queue
 622  * It is yet unclear what lock to use to protect this operation, the
 623  * discipline specific functions will determine and grab it
 624  */
 625 int
 626 altq_remove(struct pf_altq *a)
 627 {
 628         int error = 0;
 629
 630         if (a->qname[0] != 0)
 631                 return (altq_remove_queue(a));
 632
 633         switch (a->scheduler) {
 634 #ifdef ALTQ_CBQ
 635         case ALTQT_CBQ:
 636                 error = cbq_remove_altq(a);
 637                 break;
 638 #endif
 639 #ifdef ALTQ_PRIQ
 640         case ALTQT_PRIQ:
 641                 error = priq_remove_altq(a);
 642                 break;
 643 #endif
 644 #ifdef ALTQ_HFSC
 645         case ALTQT_HFSC:
 646                 error = hfsc_remove_altq(a);
 647                 break;
 648 #endif
 649         default:
 650                 error = ENXIO;
 651         }
 652
 653         return (error);
 654 }
 655
 656 /*
 657  * add a queue to the discipline
 658  * It is yet unclear what lock to use to protect this operation, the
 659  * discipline specific functions will determine and grab it
 660  */
 661 int
 662 altq_add_queue(struct pf_altq *a)
 663 {
 664         int error = 0;
 665
 666         switch (a->scheduler) {
 667 #ifdef ALTQ_CBQ
 668         case ALTQT_CBQ:
 669                 error = cbq_add_queue(a);
 670                 break;
 671 #endif
 672 #ifdef ALTQ_PRIQ
 673         case ALTQT_PRIQ:
 674                 error = priq_add_queue(a);
 675                 break;
 676 #endif
 677 #ifdef ALTQ_HFSC
 678         case ALTQT_HFSC:
 679                 error = hfsc_add_queue(a);
 680                 break;
 681 #endif
 682         default:
 683                 error = ENXIO;
 684         }
 685
 686         return (error);
 687 }
 688
 689 /*
 690  * remove a queue from the discipline
 691  * It is yet unclear what lock to use to protect this operation, the
 692  * discipline specific functions will determine and grab it
 693  */
 694 int
 695 altq_remove_queue(struct pf_altq *a)
 696 {
 697         int error = 0;
 698
 699         switch (a->scheduler) {
 700 #ifdef ALTQ_CBQ
 701         case ALTQT_CBQ:
 702                 error = cbq_remove_queue(a);
 703                 break;
 704 #endif
 705 #ifdef ALTQ_PRIQ
 706         case ALTQT_PRIQ:
 707                 error = priq_remove_queue(a);
 708                 break;
 709 #endif
 710 #ifdef ALTQ_HFSC
 711         case ALTQT_HFSC:
 712                 error = hfsc_remove_queue(a);
 713                 break;
 714 #endif
 715         default:
 716                 error = ENXIO;
 717         }
 718
 719         return (error);
 720 }
 721
 722 /*
 723  * get queue statistics
 724  * Locking is done in the discipline specific functions with regards to
 725  * copyout operations, also it is not yet clear which lock to use.
 726  */
 727 int
 728 altq_getqstats(struct pf_altq *a, void *ubuf, int *nbytes)
 729 {
 730         int error = 0;
 731
 732         switch (a->scheduler) {
 733 #ifdef ALTQ_CBQ
 734         case ALTQT_CBQ:
 735                 error = cbq_getqstats(a, ubuf, nbytes);
 736                 break;
 737 #endif
 738 #ifdef ALTQ_PRIQ
 739         case ALTQT_PRIQ:
 740                 error = priq_getqstats(a, ubuf, nbytes);
 741                 break;
 742 #endif
 743 #ifdef ALTQ_HFSC
 744         case ALTQT_HFSC:
 745                 error = hfsc_getqstats(a, ubuf, nbytes);
 746                 break;
 747 #endif
 748         default:
 749                 error = ENXIO;
 750         }
 751
 752         return (error);
 753 }
 754
 755 /*
 756  * read and write diffserv field in IPv4 or IPv6 header
 757  */
 758 u_int8_t
 759 read_dsfield(m, pktattr)
 760         struct mbuf *m;
 761         struct altq_pktattr *pktattr;
 762 {
 763         struct mbuf *m0;
 764         u_int8_t ds_field = 0;
 765
 766         if (pktattr == NULL ||
 767             (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6))
 768                 return ((u_int8_t)0);
 769
 770         /* verify that pattr_hdr is within the mbuf data */
 771         for (m0 = m; m0 != NULL; m0 = m0->m_next)
 772                 if ((pktattr->pattr_hdr >= m0->m_data) &&
 773                     (pktattr->pattr_hdr < m0->m_data + m0->m_len))
 774                         break;
 775         if (m0 == NULL) {
 776                 /* ick, pattr_hdr is stale */
 777                 pktattr->pattr_af = AF_UNSPEC;
 778 #ifdef ALTQ_DEBUG
 779                 printf("read_dsfield: can't locate header!\n");
 780 #endif
 781                 return ((u_int8_t)0);
 782         }
 783
 784         if (pktattr->pattr_af == AF_INET) {
 785                 struct ip *ip = (struct ip *)pktattr->pattr_hdr;
 786
 787                 if (ip->ip_v != 4)
 788                         return ((u_int8_t)0);   /* version mismatch! */
 789                 ds_field = ip->ip_tos;
 790         }
 791 #ifdef INET6
 792         else if (pktattr->pattr_af == AF_INET6) {
 793                 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr;
 794                 u_int32_t flowlabel;
 795
 796                 flowlabel = ntohl(ip6->ip6_flow);
 797                 if ((flowlabel >> 28) != 6)
 798                         return ((u_int8_t)0);   /* version mismatch! */
 799                 ds_field = (flowlabel >> 20) & 0xff;
 800         }
 801 #endif
 802         return (ds_field);
 803 }
 804
 805 void
 806 write_dsfield(struct mbuf *m, struct altq_pktattr *pktattr, u_int8_t dsfield)
 807 {
 808         struct mbuf *m0;
 809
 810         if (pktattr == NULL ||
 811             (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6))
 812                 return;
 813
 814         /* verify that pattr_hdr is within the mbuf data */
 815         for (m0 = m; m0 != NULL; m0 = m0->m_next)
 816                 if ((pktattr->pattr_hdr >= m0->m_data) &&
 817                     (pktattr->pattr_hdr < m0->m_data + m0->m_len))
 818                         break;
 819         if (m0 == NULL) {
 820                 /* ick, pattr_hdr is stale */
 821                 pktattr->pattr_af = AF_UNSPEC;
 822 #ifdef ALTQ_DEBUG
 823                 printf("write_dsfield: can't locate header!\n");
 824 #endif
 825                 return;
 826         }
 827
 828         if (pktattr->pattr_af == AF_INET) {
 829                 struct ip *ip = (struct ip *)pktattr->pattr_hdr;
 830                 u_int8_t old;
 831                 int32_t sum;
 832
 833                 if (ip->ip_v != 4)
 834                         return;         /* version mismatch! */
 835                 old = ip->ip_tos;
 836                 dsfield |= old & 3;     /* leave CU bits */
 837                 if (old == dsfield)
 838                         return;
 839                 ip->ip_tos = dsfield;
 840                 /*
 841                  * update checksum (from RFC1624)
 842                  *         HC' = ~(~HC + ~m + m')
 843                  */
 844                 sum = ~ntohs(ip->ip_sum) & 0xffff;
 845                 sum += 0xff00 + (~old & 0xff) + dsfield;
 846                 sum = (sum >> 16) + (sum & 0xffff);
 847                 sum += (sum >> 16);  /* add carry */
 848
 849                 ip->ip_sum = htons(~sum & 0xffff);
 850         }
 851 #ifdef INET6
 852         else if (pktattr->pattr_af == AF_INET6) {
 853                 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr;
 854                 u_int32_t flowlabel;
 855
 856                 flowlabel = ntohl(ip6->ip6_flow);
 857                 if ((flowlabel >> 28) != 6)
 858                         return;         /* version mismatch! */
 859                 flowlabel = (flowlabel & 0xf03fffff) | (dsfield << 20);
 860                 ip6->ip6_flow = htonl(flowlabel);
 861         }
 862 #endif
 863         return;
 864 }
 865
 866
 867 /*
 868  * high resolution clock support taking advantage of a machine dependent
 869  * high resolution time counter (e.g., timestamp counter of intel pentium).
 870  * we assume
 871  *  - 64-bit-long monotonically-increasing counter
 872  *  - frequency range is 100M-4GHz (CPU speed)
 873  */
 874 /* if pcc is not available or disabled, emulate 256MHz using microtime() */
 875 #define MACHCLK_SHIFT   8
 876
 877 int machclk_usepcc;
 878 u_int32_t machclk_freq;
 879 u_int32_t machclk_per_tick;
 880
 881 #if defined(__i386__) && defined(__NetBSD__)
 882 extern u_int64_t cpu_tsc_freq;
 883 #endif
 884
 885 #if (__FreeBSD_version >= 700035)
 886 /* Update TSC freq with the value indicated by the caller. */
 887 static void
 888 tsc_freq_changed(void *arg, const struct cf_level *level, int status)
 889 {
 890         /* If there was an error during the transition, don't do anything. */
 891         if (status != 0)
 892                 return;
 893
 894 #if (__FreeBSD_version >= 701102) && (defined(__amd64__) || defined(__i386__))
 895         /* If TSC is P-state invariant, don't do anything. */
 896         if (tsc_is_invariant)
 897                 return;
 898 #endif
 899
 900         /* Total setting for this level gives the new frequency in MHz. */
 901         init_machclk();
 902 }
 903 EVENTHANDLER_DEFINE(cpufreq_post_change, tsc_freq_changed, NULL,
 904     EVENTHANDLER_PRI_LAST);
 905 #endif /* __FreeBSD_version >= 700035 */
 906
 907 static void
 908 init_machclk_setup(void)
 909 {
 910 #if (__FreeBSD_version >= 600000)
 911         callout_init(&tbr_callout, 0);
 912 #endif
 913
 914         machclk_usepcc = 1;
 915
 916 #if (!defined(__amd64__) && !defined(__i386__)) || defined(ALTQ_NOPCC)
 917         machclk_usepcc = 0;
 918 #endif
 919 #if defined(__FreeBSD__) && defined(SMP)
 920         machclk_usepcc = 0;
 921 #endif
 922 #if defined(__NetBSD__) && defined(MULTIPROCESSOR)
 923         machclk_usepcc = 0;
 924 #endif
 925 #if defined(__amd64__) || defined(__i386__)
 926         /* check if TSC is available */
 927 #ifdef __FreeBSD__
 928         if ((cpu_feature & CPUID_TSC) == 0 ||
 929             atomic_load_acq_64(&tsc_freq) == 0)
 930 #else
 931         if ((cpu_feature & CPUID_TSC) == 0)
 932 #endif
 933                 machclk_usepcc = 0;
 934 #endif
 935 }
 936
 937 void
 938 init_machclk(void)
 939 {
 940         static int called;
 941
 942         /* Call one-time initialization function. */
 943         if (!called) {
 944                 init_machclk_setup();
 945                 called = 1;
 946         }
 947
 948         if (machclk_usepcc == 0) {
 949                 /* emulate 256MHz using microtime() */
 950                 machclk_freq = 1000000 << MACHCLK_SHIFT;
 951                 machclk_per_tick = machclk_freq / hz;
 952 #ifdef ALTQ_DEBUG
 953                 printf("altq: emulate %uHz cpu clock\n", machclk_freq);
 954 #endif
 955                 return;
 956         }
 957
 958         /*
 959          * if the clock frequency (of Pentium TSC or Alpha PCC) is
 960          * accessible, just use it.
 961          */
 962 #if defined(__amd64__) || defined(__i386__)
 963 #ifdef __FreeBSD__
 964         machclk_freq = atomic_load_acq_64(&tsc_freq);
 965 #elif defined(__NetBSD__)
 966         machclk_freq = (u_int32_t)cpu_tsc_freq;
 967 #elif defined(__OpenBSD__) && (defined(I586_CPU) || defined(I686_CPU))
 968         machclk_freq = pentium_mhz * 1000000;
 969 #endif
 970 #endif
 971
 972         /*
 973          * if we don't know the clock frequency, measure it.
 974          */
 975         if (machclk_freq == 0) {
 976                 static int      wait;
 977                 struct timeval  tv_start, tv_end;
 978                 u_int64_t       start, end, diff;
 979                 int             timo;
 980
 981                 microtime(&tv_start);
 982                 start = read_machclk();
 983                 timo = hz;      /* 1 sec */
 984                 (void)tsleep(&wait, PWAIT | PCATCH, "init_machclk", timo);
 985                 microtime(&tv_end);
 986                 end = read_machclk();
 987                 diff = (u_int64_t)(tv_end.tv_sec - tv_start.tv_sec) * 1000000
 988                     + tv_end.tv_usec - tv_start.tv_usec;
 989                 if (diff != 0)
 990                         machclk_freq = (u_int)((end - start) * 1000000 / diff);
 991         }
 992
 993         machclk_per_tick = machclk_freq / hz;
 994
 995 #ifdef ALTQ_DEBUG
 996         printf("altq: CPU clock: %uHz\n", machclk_freq);
 997 #endif
 998 }
 999
1000 #if defined(__OpenBSD__) && defined(__i386__)
1001 static __inline u_int64_t
1002 rdtsc(void)
1003 {
1004         u_int64_t rv;
1005         __asm __volatile(".byte 0x0f, 0x31" : "=A" (rv));
1006         return (rv);
1007 }
1008 #endif /* __OpenBSD__ && __i386__ */
1009
1010 u_int64_t
1011 read_machclk(void)
1012 {
1013         u_int64_t val;
1014
1015         if (machclk_usepcc) {
1016 #if defined(__amd64__) || defined(__i386__)
1017                 val = rdtsc();
1018 #else
1019                 panic("read_machclk");
1020 #endif
1021         } else {
1022                 struct timeval tv;
1023
1024                 microtime(&tv);
1025                 val = (((u_int64_t)(tv.tv_sec - boottime.tv_sec) * 1000000
1026                     + tv.tv_usec) << MACHCLK_SHIFT);
1027         }
1028         return (val);
1029 }
1030
1031 #ifdef ALTQ3_CLFIER_COMPAT
1032
1033 #ifndef IPPROTO_ESP
1034 #define IPPROTO_ESP     50              /* encapsulating security payload */
1035 #endif
1036 #ifndef IPPROTO_AH
1037 #define IPPROTO_AH      51              /* authentication header */
1038 #endif
1039
1040 /*
1041  * extract flow information from a given packet.
1042  * filt_mask shows flowinfo fields required.
1043  * we assume the ip header is in one mbuf, and addresses and ports are
1044  * in network byte order.
1045  */
1046 int
1047 altq_extractflow(m, af, flow, filt_bmask)
1048         struct mbuf *m;
1049         int af;
1050         struct flowinfo *flow;
1051         u_int32_t       filt_bmask;
1052 {
1053
1054         switch (af) {
1055         case PF_INET: {
1056                 struct flowinfo_in *fin;
1057                 struct ip *ip;
1058
1059                 ip = mtod(m, struct ip *);
1060
1061                 if (ip->ip_v != 4)
1062                         break;
1063
1064                 fin = (struct flowinfo_in *)flow;
1065                 fin->fi_len = sizeof(struct flowinfo_in);
1066                 fin->fi_family = AF_INET;
1067
1068                 fin->fi_proto = ip->ip_p;
1069                 fin->fi_tos = ip->ip_tos;
1070
1071                 fin->fi_src.s_addr = ip->ip_src.s_addr;
1072                 fin->fi_dst.s_addr = ip->ip_dst.s_addr;
1073
1074                 if (filt_bmask & FIMB4_PORTS)
1075                         /* if port info is required, extract port numbers */
1076                         extract_ports4(m, ip, fin);
1077                 else {
1078                         fin->fi_sport = 0;
1079                         fin->fi_dport = 0;
1080                         fin->fi_gpi = 0;
1081                 }
1082                 return (1);
1083         }
1084
1085 #ifdef INET6
1086         case PF_INET6: {
1087                 struct flowinfo_in6 *fin6;
1088                 struct ip6_hdr *ip6;
1089
1090                 ip6 = mtod(m, struct ip6_hdr *);
1091                 /* should we check the ip version? */
1092
1093                 fin6 = (struct flowinfo_in6 *)flow;
1094                 fin6->fi6_len = sizeof(struct flowinfo_in6);
1095                 fin6->fi6_family = AF_INET6;
1096
1097                 fin6->fi6_proto = ip6->ip6_nxt;
1098                 fin6->fi6_tclass   = (ntohl(ip6->ip6_flow) >> 20) & 0xff;
1099
1100                 fin6->fi6_flowlabel = ip6->ip6_flow & htonl(0x000fffff);
1101                 fin6->fi6_src = ip6->ip6_src;
1102                 fin6->fi6_dst = ip6->ip6_dst;
1103
1104                 if ((filt_bmask & FIMB6_PORTS) ||
1105                     ((filt_bmask & FIMB6_PROTO)
1106                      && ip6->ip6_nxt > IPPROTO_IPV6))
1107                         /*
1108                          * if port info is required, or proto is required
1109                          * but there are option headers, extract port
1110                          * and protocol numbers.
1111                          */
1112                         extract_ports6(m, ip6, fin6);
1113                 else {
1114                         fin6->fi6_sport = 0;
1115                         fin6->fi6_dport = 0;
1116                         fin6->fi6_gpi = 0;
1117                 }
1118                 return (1);
1119         }
1120 #endif /* INET6 */
1121
1122         default:
1123                 break;
1124         }
1125
1126         /* failed */
1127         flow->fi_len = sizeof(struct flowinfo);
1128         flow->fi_family = AF_UNSPEC;
1129         return (0);
1130 }
1131
1132 /*
1133  * helper routine to extract port numbers
1134  */
1135 /* structure for ipsec and ipv6 option header template */
1136 struct _opt6 {
1137         u_int8_t        opt6_nxt;       /* next header */
1138         u_int8_t        opt6_hlen;      /* header extension length */
1139         u_int16_t       _pad;
1140         u_int32_t       ah_spi;         /* security parameter index
1141                                            for authentication header */
1142 };
1143
1144 /*
1145  * extract port numbers from a ipv4 packet.
1146  */
1147 static int
1148 extract_ports4(m, ip, fin)
1149         struct mbuf *m;
1150         struct ip *ip;
1151         struct flowinfo_in *fin;
1152 {
1153         struct mbuf *m0;
1154         u_short ip_off;
1155         u_int8_t proto;
1156         int     off;
1157
1158         fin->fi_sport = 0;
1159         fin->fi_dport = 0;
1160         fin->fi_gpi = 0;
1161
1162         ip_off = ntohs(ip->ip_off);
1163         /* if it is a fragment, try cached fragment info */
1164         if (ip_off & IP_OFFMASK) {
1165                 ip4f_lookup(ip, fin);
1166                 return (1);
1167         }
1168
1169         /* locate the mbuf containing the protocol header */
1170         for (m0 = m; m0 != NULL; m0 = m0->m_next)
1171                 if (((caddr_t)ip >= m0->m_data) &&
1172                     ((caddr_t)ip < m0->m_data + m0->m_len))
1173                         break;
1174         if (m0 == NULL) {
1175 #ifdef ALTQ_DEBUG
1176                 printf("extract_ports4: can't locate header! ip=%p\n", ip);
1177 #endif
1178                 return (0);
1179         }
1180         off = ((caddr_t)ip - m0->m_data) + (ip->ip_hl << 2);
1181         proto = ip->ip_p;
1182
1183 #ifdef ALTQ_IPSEC
1184  again:
1185 #endif
1186         while (off >= m0->m_len) {
1187                 off -= m0->m_len;
1188                 m0 = m0->m_next;
1189                 if (m0 == NULL)
1190                         return (0);  /* bogus ip_hl! */
1191         }
1192         if (m0->m_len < off + 4)
1193                 return (0);
1194
1195         switch (proto) {
1196         case IPPROTO_TCP:
1197         case IPPROTO_UDP: {
1198                 struct udphdr *udp;
1199
1200                 udp = (struct udphdr *)(mtod(m0, caddr_t) + off);
1201                 fin->fi_sport = udp->uh_sport;
1202                 fin->fi_dport = udp->uh_dport;
1203                 fin->fi_proto = proto;
1204                 }
1205                 break;
1206
1207 #ifdef ALTQ_IPSEC
1208         case IPPROTO_ESP:
1209                 if (fin->fi_gpi == 0){
1210                         u_int32_t *gpi;
1211
1212                         gpi = (u_int32_t *)(mtod(m0, caddr_t) + off);
1213                         fin->fi_gpi   = *gpi;
1214                 }
1215                 fin->fi_proto = proto;
1216                 break;
1217
1218         case IPPROTO_AH: {
1219                         /* get next header and header length */
1220                         struct _opt6 *opt6;
1221
1222                         opt6 = (struct _opt6 *)(mtod(m0, caddr_t) + off);
1223                         proto = opt6->opt6_nxt;
1224                         off += 8 + (opt6->opt6_hlen * 4);
1225                         if (fin->fi_gpi == 0 && m0->m_len >= off + 8)
1226                                 fin->fi_gpi = opt6->ah_spi;
1227                 }
1228                 /* goto the next header */
1229                 goto again;
1230 #endif  /* ALTQ_IPSEC */
1231
1232         default:
1233                 fin->fi_proto = proto;
1234                 return (0);
1235         }
1236
1237         /* if this is a first fragment, cache it. */
1238         if (ip_off & IP_MF)
1239                 ip4f_cache(ip, fin);
1240
1241         return (1);
1242 }
1243
1244 #ifdef INET6
1245 static int
1246 extract_ports6(m, ip6, fin6)
1247         struct mbuf *m;
1248         struct ip6_hdr *ip6;
1249         struct flowinfo_in6 *fin6;
1250 {
1251         struct mbuf *m0;
1252         int     off;
1253         u_int8_t proto;
1254
1255         fin6->fi6_gpi   = 0;
1256         fin6->fi6_sport = 0;
1257         fin6->fi6_dport = 0;
1258
1259         /* locate the mbuf containing the protocol header */
1260         for (m0 = m; m0 != NULL; m0 = m0->m_next)
1261                 if (((caddr_t)ip6 >= m0->m_data) &&
1262                     ((caddr_t)ip6 < m0->m_data + m0->m_len))
1263                         break;
1264         if (m0 == NULL) {
1265 #ifdef ALTQ_DEBUG
1266                 printf("extract_ports6: can't locate header! ip6=%p\n", ip6);
1267 #endif
1268                 return (0);
1269         }
1270         off = ((caddr_t)ip6 - m0->m_data) + sizeof(struct ip6_hdr);
1271
1272         proto = ip6->ip6_nxt;
1273         do {
1274                 while (off >= m0->m_len) {
1275                         off -= m0->m_len;
1276                         m0 = m0->m_next;
1277                         if (m0 == NULL)
1278                                 return (0);
1279                 }
1280                 if (m0->m_len < off + 4)
1281                         return (0);
1282
1283                 switch (proto) {
1284                 case IPPROTO_TCP:
1285                 case IPPROTO_UDP: {
1286                         struct udphdr *udp;
1287
1288                         udp = (struct udphdr *)(mtod(m0, caddr_t) + off);
1289                         fin6->fi6_sport = udp->uh_sport;
1290                         fin6->fi6_dport = udp->uh_dport;
1291                         fin6->fi6_proto = proto;
1292                         }
1293                         return (1);
1294
1295                 case IPPROTO_ESP:
1296                         if (fin6->fi6_gpi == 0) {
1297                                 u_int32_t *gpi;
1298
1299                                 gpi = (u_int32_t *)(mtod(m0, caddr_t) + off);
1300                                 fin6->fi6_gpi   = *gpi;
1301                         }
1302                         fin6->fi6_proto = proto;
1303                         return (1);
1304
1305                 case IPPROTO_AH: {
1306                         /* get next header and header length */
1307                         struct _opt6 *opt6;
1308
1309                         opt6 = (struct _opt6 *)(mtod(m0, caddr_t) + off);
1310                         if (fin6->fi6_gpi == 0 && m0->m_len >= off + 8)
1311                                 fin6->fi6_gpi = opt6->ah_spi;
1312                         proto = opt6->opt6_nxt;
1313                         off += 8 + (opt6->opt6_hlen * 4);
1314                         /* goto the next header */
1315                         break;
1316                         }
1317
1318                 case IPPROTO_HOPOPTS:
1319                 case IPPROTO_ROUTING:
1320                 case IPPROTO_DSTOPTS: {
1321                         /* get next header and header length */
1322                         struct _opt6 *opt6;
1323
1324                         opt6 = (struct _opt6 *)(mtod(m0, caddr_t) + off);
1325                         proto = opt6->opt6_nxt;
1326                         off += (opt6->opt6_hlen + 1) * 8;
1327                         /* goto the next header */
1328                         break;
1329                         }
1330
1331                 case IPPROTO_FRAGMENT:
1332                         /* ipv6 fragmentations are not supported yet */
1333                 default:
1334                         fin6->fi6_proto = proto;
1335                         return (0);
1336                 }
1337         } while (1);
1338         /*NOTREACHED*/
1339 }
1340 #endif /* INET6 */
1341
1342 /*
1343  * altq common classifier
1344  */
1345 int
1346 acc_add_filter(classifier, filter, class, phandle)
1347         struct acc_classifier *classifier;
1348         struct flow_filter *filter;
1349         void    *class;
1350         u_long  *phandle;
1351 {
1352         struct acc_filter *afp, *prev, *tmp;
1353         int     i, s;
1354
1355 #ifdef INET6
1356         if (filter->ff_flow.fi_family != AF_INET &&
1357             filter->ff_flow.fi_family != AF_INET6)
1358                 return (EINVAL);
1359 #else
1360         if (filter->ff_flow.fi_family != AF_INET)
1361                 return (EINVAL);
1362 #endif
1363
1364         afp = malloc(sizeof(struct acc_filter),
1365                M_DEVBUF, M_WAITOK);
1366         if (afp == NULL)
1367                 return (ENOMEM);
1368         bzero(afp, sizeof(struct acc_filter));
1369
1370         afp->f_filter = *filter;
1371         afp->f_class = class;
1372
1373         i = ACC_WILDCARD_INDEX;
1374         if (filter->ff_flow.fi_family == AF_INET) {
1375                 struct flow_filter *filter4 = &afp->f_filter;
1376
1377                 /*
1378                  * if address is 0, it's a wildcard.  if address mask
1379                  * isn't set, use full mask.
1380                  */
1381                 if (filter4->ff_flow.fi_dst.s_addr == 0)
1382                         filter4->ff_mask.mask_dst.s_addr = 0;
1383                 else if (filter4->ff_mask.mask_dst.s_addr == 0)
1384                         filter4->ff_mask.mask_dst.s_addr = 0xffffffff;
1385                 if (filter4->ff_flow.fi_src.s_addr == 0)
1386                         filter4->ff_mask.mask_src.s_addr = 0;
1387                 else if (filter4->ff_mask.mask_src.s_addr == 0)
1388                         filter4->ff_mask.mask_src.s_addr = 0xffffffff;
1389
1390                 /* clear extra bits in addresses  */
1391                    filter4->ff_flow.fi_dst.s_addr &=
1392                        filter4->ff_mask.mask_dst.s_addr;
1393                    filter4->ff_flow.fi_src.s_addr &=
1394                        filter4->ff_mask.mask_src.s_addr;
1395
1396                 /*
1397                  * if dst address is a wildcard, use hash-entry
1398                  * ACC_WILDCARD_INDEX.
1399                  */
1400                 if (filter4->ff_mask.mask_dst.s_addr != 0xffffffff)
1401                         i = ACC_WILDCARD_INDEX;
1402                 else
1403                         i = ACC_GET_HASH_INDEX(filter4->ff_flow.fi_dst.s_addr);
1404         }
1405 #ifdef INET6
1406         else if (filter->ff_flow.fi_family == AF_INET6) {
1407                 struct flow_filter6 *filter6 =
1408                         (struct flow_filter6 *)&afp->f_filter;
1409 #ifndef IN6MASK0 /* taken from kame ipv6 */
1410 #define IN6MASK0        {{{ 0, 0, 0, 0 }}}
1411 #define IN6MASK128      {{{ 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff }}}
1412                 const struct in6_addr in6mask0 = IN6MASK0;
1413                 const struct in6_addr in6mask128 = IN6MASK128;
1414 #endif
1415
1416                 if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_flow6.fi6_dst))
1417                         filter6->ff_mask6.mask6_dst = in6mask0;
1418                 else if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_mask6.mask6_dst))
1419                         filter6->ff_mask6.mask6_dst = in6mask128;
1420                 if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_flow6.fi6_src))
1421                         filter6->ff_mask6.mask6_src = in6mask0;
1422                 else if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_mask6.mask6_src))
1423                         filter6->ff_mask6.mask6_src = in6mask128;
1424
1425                 /* clear extra bits in addresses  */
1426                 for (i = 0; i < 16; i++)
1427                         filter6->ff_flow6.fi6_dst.s6_addr[i] &=
1428                             filter6->ff_mask6.mask6_dst.s6_addr[i];
1429                 for (i = 0; i < 16; i++)
1430                         filter6->ff_flow6.fi6_src.s6_addr[i] &=
1431                             filter6->ff_mask6.mask6_src.s6_addr[i];
1432
1433                 if (filter6->ff_flow6.fi6_flowlabel == 0)
1434                         i = ACC_WILDCARD_INDEX;
1435                 else
1436                         i = ACC_GET_HASH_INDEX(filter6->ff_flow6.fi6_flowlabel);
1437         }
1438 #endif /* INET6 */
1439
1440         afp->f_handle = get_filt_handle(classifier, i);
1441
1442         /* update filter bitmask */
1443         afp->f_fbmask = filt2fibmask(filter);
1444         classifier->acc_fbmask |= afp->f_fbmask;
1445
1446         /*
1447          * add this filter to the filter list.
1448          * filters are ordered from the highest rule number.
1449          */
1450 #ifdef __NetBSD__
1451         s = splnet();
1452 #else
1453         s = splimp();
1454 #endif
1455         prev = NULL;
1456         LIST_FOREACH(tmp, &classifier->acc_filters[i], f_chain) {
1457                 if (tmp->f_filter.ff_ruleno > afp->f_filter.ff_ruleno)
1458                         prev = tmp;
1459                 else
1460                         break;
1461         }
1462         if (prev == NULL)
1463                 LIST_INSERT_HEAD(&classifier->acc_filters[i], afp, f_chain);
1464         else
1465                 LIST_INSERT_AFTER(prev, afp, f_chain);
1466         splx(s);
1467
1468         *phandle = afp->f_handle;
1469         return (0);
1470 }
1471
1472 int
1473 acc_delete_filter(classifier, handle)
1474         struct acc_classifier *classifier;
1475         u_long handle;
1476 {
1477         struct acc_filter *afp;
1478         int     s;
1479
1480         if ((afp = filth_to_filtp(classifier, handle)) == NULL)
1481                 return (EINVAL);
1482
1483 #ifdef __NetBSD__
1484         s = splnet();
1485 #else
1486         s = splimp();
1487 #endif
1488         LIST_REMOVE(afp, f_chain);
1489         splx(s);
1490
1491         free(afp, M_DEVBUF);
1492
1493         /* todo: update filt_bmask */
1494
1495         return (0);
1496 }
1497
1498 /*
1499  * delete filters referencing to the specified class.
1500  * if the all flag is not 0, delete all the filters.
1501  */
1502 int
1503 acc_discard_filters(classifier, class, all)
1504         struct acc_classifier *classifier;
1505         void    *class;
1506         int     all;
1507 {
1508         struct acc_filter *afp;
1509         int     i, s;
1510
1511 #ifdef __NetBSD__
1512         s = splnet();
1513 #else
1514         s = splimp();
1515 #endif
1516         for (i = 0; i < ACC_FILTER_TABLESIZE; i++) {
1517                 do {
1518                         LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
1519                                 if (all || afp->f_class == class) {
1520                                         LIST_REMOVE(afp, f_chain);
1521                                         free(afp, M_DEVBUF);
1522                                         /* start again from the head */
1523                                         break;
1524                                 }
1525                 } while (afp != NULL);
1526         }
1527         splx(s);
1528
1529         if (all)
1530                 classifier->acc_fbmask = 0;
1531
1532         return (0);
1533 }
1534
1535 void *
1536 acc_classify(clfier, m, af)
1537         void *clfier;
1538         struct mbuf *m;
1539         int af;
1540 {
1541         struct acc_classifier *classifier;
1542         struct flowinfo flow;
1543         struct acc_filter *afp;
1544         int     i;
1545
1546         classifier = (struct acc_classifier *)clfier;
1547         altq_extractflow(m, af, &flow, classifier->acc_fbmask);
1548
1549         if (flow.fi_family == AF_INET) {
1550                 struct flowinfo_in *fp = (struct flowinfo_in *)&flow;
1551
1552                 if ((classifier->acc_fbmask & FIMB4_ALL) == FIMB4_TOS) {
1553                         /* only tos is used */
1554                         LIST_FOREACH(afp,
1555                                  &classifier->acc_filters[ACC_WILDCARD_INDEX],
1556                                  f_chain)
1557                                 if (apply_tosfilter4(afp->f_fbmask,
1558                                                      &afp->f_filter, fp))
1559                                         /* filter matched */
1560                                         return (afp->f_class);
1561                 } else if ((classifier->acc_fbmask &
1562                         (~(FIMB4_PROTO|FIMB4_SPORT|FIMB4_DPORT) & FIMB4_ALL))
1563                     == 0) {
1564                         /* only proto and ports are used */
1565                         LIST_FOREACH(afp,
1566                                  &classifier->acc_filters[ACC_WILDCARD_INDEX],
1567                                  f_chain)
1568                                 if (apply_ppfilter4(afp->f_fbmask,
1569                                                     &afp->f_filter, fp))
1570                                         /* filter matched */
1571                                         return (afp->f_class);
1572                 } else {
1573                         /* get the filter hash entry from its dest address */
1574                         i = ACC_GET_HASH_INDEX(fp->fi_dst.s_addr);
1575                         do {
1576                                 /*
1577                                  * go through this loop twice.  first for dst
1578                                  * hash, second for wildcards.
1579                                  */
1580                                 LIST_FOREACH(afp, &classifier->acc_filters[i],
1581                                              f_chain)
1582                                         if (apply_filter4(afp->f_fbmask,
1583                                                           &afp->f_filter, fp))
1584                                                 /* filter matched */
1585                                                 return (afp->f_class);
1586
1587                                 /*
1588                                  * check again for filters with a dst addr
1589                                  * wildcard.
1590                                  * (daddr == 0 || dmask != 0xffffffff).
1591                                  */
1592                                 if (i != ACC_WILDCARD_INDEX)
1593                                         i = ACC_WILDCARD_INDEX;
1594                                 else
1595                                         break;
1596                         } while (1);
1597                 }
1598         }
1599 #ifdef INET6
1600         else if (flow.fi_family == AF_INET6) {
1601                 struct flowinfo_in6 *fp6 = (struct flowinfo_in6 *)&flow;
1602
1603                 /* get the filter hash entry from its flow ID */
1604                 if (fp6->fi6_flowlabel != 0)
1605                         i = ACC_GET_HASH_INDEX(fp6->fi6_flowlabel);
1606                 else
1607                         /* flowlable can be zero */
1608                         i = ACC_WILDCARD_INDEX;
1609
1610                 /* go through this loop twice.  first for flow hash, second
1611                    for wildcards. */
1612                 do {
1613                         LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
1614                                 if (apply_filter6(afp->f_fbmask,
1615                                         (struct flow_filter6 *)&afp->f_filter,
1616                                         fp6))
1617                                         /* filter matched */
1618                                         return (afp->f_class);
1619
1620                         /*
1621                          * check again for filters with a wildcard.
1622                          */
1623                         if (i != ACC_WILDCARD_INDEX)
1624                                 i = ACC_WILDCARD_INDEX;
1625                         else
1626                                 break;
1627                 } while (1);
1628         }
1629 #endif /* INET6 */
1630
1631         /* no filter matched */
1632         return (NULL);
1633 }
1634
1635 static int
1636 apply_filter4(fbmask, filt, pkt)
1637         u_int32_t       fbmask;
1638         struct flow_filter *filt;
1639         struct flowinfo_in *pkt;
1640 {
1641         if (filt->ff_flow.fi_family != AF_INET)
1642                 return (0);
1643         if ((fbmask & FIMB4_SPORT) && filt->ff_flow.fi_sport != pkt->fi_sport)
1644                 return (0);
1645         if ((fbmask & FIMB4_DPORT) && filt->ff_flow.fi_dport != pkt->fi_dport)
1646                 return (0);
1647         if ((fbmask & FIMB4_DADDR) &&
1648             filt->ff_flow.fi_dst.s_addr !=
1649             (pkt->fi_dst.s_addr & filt->ff_mask.mask_dst.s_addr))
1650                 return (0);
1651         if ((fbmask & FIMB4_SADDR) &&
1652             filt->ff_flow.fi_src.s_addr !=
1653             (pkt->fi_src.s_addr & filt->ff_mask.mask_src.s_addr))
1654                 return (0);
1655         if ((fbmask & FIMB4_PROTO) && filt->ff_flow.fi_proto != pkt->fi_proto)
1656                 return (0);
1657         if ((fbmask & FIMB4_TOS) && filt->ff_flow.fi_tos !=
1658             (pkt->fi_tos & filt->ff_mask.mask_tos))
1659                 return (0);
1660         if ((fbmask & FIMB4_GPI) && filt->ff_flow.fi_gpi != (pkt->fi_gpi))
1661                 return (0);
1662         /* match */
1663         return (1);
1664 }
1665
1666 /*
1667  * filter matching function optimized for a common case that checks
1668  * only protocol and port numbers
1669  */
1670 static int
1671 apply_ppfilter4(fbmask, filt, pkt)
1672         u_int32_t       fbmask;
1673         struct flow_filter *filt;
1674         struct flowinfo_in *pkt;
1675 {
1676         if (filt->ff_flow.fi_family != AF_INET)
1677                 return (0);
1678         if ((fbmask & FIMB4_SPORT) && filt->ff_flow.fi_sport != pkt->fi_sport)
1679                 return (0);
1680         if ((fbmask & FIMB4_DPORT) && filt->ff_flow.fi_dport != pkt->fi_dport)
1681                 return (0);
1682         if ((fbmask & FIMB4_PROTO) && filt->ff_flow.fi_proto != pkt->fi_proto)
1683                 return (0);
1684         /* match */
1685         return (1);
1686 }
1687
1688 /*
1689  * filter matching function only for tos field.
1690  */
1691 static int
1692 apply_tosfilter4(fbmask, filt, pkt)
1693         u_int32_t       fbmask;
1694         struct flow_filter *filt;
1695         struct flowinfo_in *pkt;
1696 {
1697         if (filt->ff_flow.fi_family != AF_INET)
1698                 return (0);
1699         if ((fbmask & FIMB4_TOS) && filt->ff_flow.fi_tos !=
1700             (pkt->fi_tos & filt->ff_mask.mask_tos))
1701                 return (0);
1702         /* match */
1703         return (1);
1704 }
1705
1706 #ifdef INET6
1707 static int
1708 apply_filter6(fbmask, filt, pkt)
1709         u_int32_t       fbmask;
1710         struct flow_filter6 *filt;
1711         struct flowinfo_in6 *pkt;
1712 {
1713         int i;
1714
1715         if (filt->ff_flow6.fi6_family != AF_INET6)
1716                 return (0);
1717         if ((fbmask & FIMB6_FLABEL) &&
1718             filt->ff_flow6.fi6_flowlabel != pkt->fi6_flowlabel)
1719                 return (0);
1720         if ((fbmask & FIMB6_PROTO) &&
1721             filt->ff_flow6.fi6_proto != pkt->fi6_proto)
1722                 return (0);
1723         if ((fbmask & FIMB6_SPORT) &&
1724             filt->ff_flow6.fi6_sport != pkt->fi6_sport)
1725                 return (0);
1726         if ((fbmask & FIMB6_DPORT) &&
1727             filt->ff_flow6.fi6_dport != pkt->fi6_dport)
1728                 return (0);
1729         if (fbmask & FIMB6_SADDR) {
1730                 for (i = 0; i < 4; i++)
1731                         if (filt->ff_flow6.fi6_src.s6_addr32[i] !=
1732                             (pkt->fi6_src.s6_addr32[i] &
1733                              filt->ff_mask6.mask6_src.s6_addr32[i]))
1734                                 return (0);
1735         }
1736         if (fbmask & FIMB6_DADDR) {
1737                 for (i = 0; i < 4; i++)
1738                         if (filt->ff_flow6.fi6_dst.s6_addr32[i] !=
1739                             (pkt->fi6_dst.s6_addr32[i] &
1740                              filt->ff_mask6.mask6_dst.s6_addr32[i]))
1741                                 return (0);
1742         }
1743         if ((fbmask & FIMB6_TCLASS) &&
1744             filt->ff_flow6.fi6_tclass !=
1745             (pkt->fi6_tclass & filt->ff_mask6.mask6_tclass))
1746                 return (0);
1747         if ((fbmask & FIMB6_GPI) &&
1748             filt->ff_flow6.fi6_gpi != pkt->fi6_gpi)
1749                 return (0);
1750         /* match */
1751         return (1);
1752 }
1753 #endif /* INET6 */
1754
1755 /*
1756  *  filter handle:
1757  *      bit 20-28: index to the filter hash table
1758  *      bit  0-19: unique id in the hash bucket.
1759  */
1760 static u_long
1761 get_filt_handle(classifier, i)
1762         struct acc_classifier *classifier;
1763         int     i;
1764 {
1765         static u_long handle_number = 1;
1766         u_long  handle;
1767         struct acc_filter *afp;
1768
1769         while (1) {
1770                 handle = handle_number++ & 0x000fffff;
1771
1772                 if (LIST_EMPTY(&classifier->acc_filters[i]))
1773                         break;
1774
1775                 LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
1776                         if ((afp->f_handle & 0x000fffff) == handle)
1777                                 break;
1778                 if (afp == NULL)
1779                         break;
1780                 /* this handle is already used, try again */
1781         }
1782
1783         return ((i << 20) | handle);
1784 }
1785
1786 /* convert filter handle to filter pointer */
1787 static struct acc_filter *
1788 filth_to_filtp(classifier, handle)
1789         struct acc_classifier *classifier;
1790         u_long handle;
1791 {
1792         struct acc_filter *afp;
1793         int     i;
1794
1795         i = ACC_GET_HINDEX(handle);
1796
1797         LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
1798                 if (afp->f_handle == handle)
1799                         return (afp);
1800
1801         return (NULL);
1802 }
1803
1804 /* create flowinfo bitmask */
1805 static u_int32_t
1806 filt2fibmask(filt)
1807         struct flow_filter *filt;
1808 {
1809         u_int32_t mask = 0;
1810 #ifdef INET6
1811         struct flow_filter6 *filt6;
1812 #endif
1813
1814         switch (filt->ff_flow.fi_family) {
1815         case AF_INET:
1816                 if (filt->ff_flow.fi_proto != 0)
1817                         mask |= FIMB4_PROTO;
1818                 if (filt->ff_flow.fi_tos != 0)
1819                         mask |= FIMB4_TOS;
1820                 if (filt->ff_flow.fi_dst.s_addr != 0)
1821                         mask |= FIMB4_DADDR;
1822                 if (filt->ff_flow.fi_src.s_addr != 0)
1823                         mask |= FIMB4_SADDR;
1824                 if (filt->ff_flow.fi_sport != 0)
1825                         mask |= FIMB4_SPORT;
1826                 if (filt->ff_flow.fi_dport != 0)
1827                         mask |= FIMB4_DPORT;
1828                 if (filt->ff_flow.fi_gpi != 0)
1829                         mask |= FIMB4_GPI;
1830                 break;
1831 #ifdef INET6
1832         case AF_INET6:
1833                 filt6 = (struct flow_filter6 *)filt;
1834
1835                 if (filt6->ff_flow6.fi6_proto != 0)
1836                         mask |= FIMB6_PROTO;
1837                 if (filt6->ff_flow6.fi6_tclass != 0)
1838                         mask |= FIMB6_TCLASS;
1839                 if (!IN6_IS_ADDR_UNSPECIFIED(&filt6->ff_flow6.fi6_dst))
1840                         mask |= FIMB6_DADDR;
1841                 if (!IN6_IS_ADDR_UNSPECIFIED(&filt6->ff_flow6.fi6_src))
1842                         mask |= FIMB6_SADDR;
1843                 if (filt6->ff_flow6.fi6_sport != 0)
1844                         mask |= FIMB6_SPORT;
1845                 if (filt6->ff_flow6.fi6_dport != 0)
1846                         mask |= FIMB6_DPORT;
1847                 if (filt6->ff_flow6.fi6_gpi != 0)
1848                         mask |= FIMB6_GPI;
1849                 if (filt6->ff_flow6.fi6_flowlabel != 0)
1850                         mask |= FIMB6_FLABEL;
1851                 break;
1852 #endif /* INET6 */
1853         }
1854         return (mask);
1855 }
1856
1857
1858 /*
1859  * helper functions to handle IPv4 fragments.
1860  * currently only in-sequence fragments are handled.
1861  *      - fragment info is cached in a LRU list.
1862  *      - when a first fragment is found, cache its flow info.
1863  *      - when a non-first fragment is found, lookup the cache.
1864  */
1865
1866 struct ip4_frag {
1867     TAILQ_ENTRY(ip4_frag) ip4f_chain;
1868     char    ip4f_valid;
1869     u_short ip4f_id;
1870     struct flowinfo_in ip4f_info;
1871 };
1872
1873 static TAILQ_HEAD(ip4f_list, ip4_frag) ip4f_list; /* IPv4 fragment cache */
1874
1875 #define IP4F_TABSIZE            16      /* IPv4 fragment cache size */
1876
1877
1878 static void
1879 ip4f_cache(ip, fin)
1880         struct ip *ip;
1881         struct flowinfo_in *fin;
1882 {
1883         struct ip4_frag *fp;
1884
1885         if (TAILQ_EMPTY(&ip4f_list)) {
1886                 /* first time call, allocate fragment cache entries. */
1887                 if (ip4f_init() < 0)
1888                         /* allocation failed! */
1889                         return;
1890         }
1891
1892         fp = ip4f_alloc();
1893         fp->ip4f_id = ip->ip_id;
1894         fp->ip4f_info.fi_proto = ip->ip_p;
1895         fp->ip4f_info.fi_src.s_addr = ip->ip_src.s_addr;
1896         fp->ip4f_info.fi_dst.s_addr = ip->ip_dst.s_addr;
1897
1898         /* save port numbers */
1899         fp->ip4f_info.fi_sport = fin->fi_sport;
1900         fp->ip4f_info.fi_dport = fin->fi_dport;
1901         fp->ip4f_info.fi_gpi   = fin->fi_gpi;
1902 }
1903
1904 static int
1905 ip4f_lookup(ip, fin)
1906         struct ip *ip;
1907         struct flowinfo_in *fin;
1908 {
1909         struct ip4_frag *fp;
1910
1911         for (fp = TAILQ_FIRST(&ip4f_list); fp != NULL && fp->ip4f_valid;
1912              fp = TAILQ_NEXT(fp, ip4f_chain))
1913                 if (ip->ip_id == fp->ip4f_id &&
1914                     ip->ip_src.s_addr == fp->ip4f_info.fi_src.s_addr &&
1915                     ip->ip_dst.s_addr == fp->ip4f_info.fi_dst.s_addr &&
1916                     ip->ip_p == fp->ip4f_info.fi_proto) {
1917
1918                         /* found the matching entry */
1919                         fin->fi_sport = fp->ip4f_info.fi_sport;
1920                         fin->fi_dport = fp->ip4f_info.fi_dport;
1921                         fin->fi_gpi   = fp->ip4f_info.fi_gpi;
1922
1923                         if ((ntohs(ip->ip_off) & IP_MF) == 0)
1924                                 /* this is the last fragment,
1925                                    release the entry. */
1926                                 ip4f_free(fp);
1927
1928                         return (1);
1929                 }
1930
1931         /* no matching entry found */
1932         return (0);
1933 }
1934
1935 static int
1936 ip4f_init(void)
1937 {
1938         struct ip4_frag *fp;
1939         int i;
1940
1941         TAILQ_INIT(&ip4f_list);
1942         for (i=0; i<IP4F_TABSIZE; i++) {
1943                 fp = malloc(sizeof(struct ip4_frag),
1944                        M_DEVBUF, M_NOWAIT);
1945                 if (fp == NULL) {
1946                         printf("ip4f_init: can't alloc %dth entry!\n", i);
1947                         if (i == 0)
1948                                 return (-1);
1949                         return (0);
1950                 }
1951                 fp->ip4f_valid = 0;
1952                 TAILQ_INSERT_TAIL(&ip4f_list, fp, ip4f_chain);
1953         }
1954         return (0);
1955 }
1956
1957 static struct ip4_frag *
1958 ip4f_alloc(void)
1959 {
1960         struct ip4_frag *fp;
1961
1962         /* reclaim an entry at the tail, put it at the head */
1963         fp = TAILQ_LAST(&ip4f_list, ip4f_list);
1964         TAILQ_REMOVE(&ip4f_list, fp, ip4f_chain);
1965         fp->ip4f_valid = 1;
1966         TAILQ_INSERT_HEAD(&ip4f_list, fp, ip4f_chain);
1967         return (fp);
1968 }
1969
1970 static void
1971 ip4f_free(fp)
1972         struct ip4_frag *fp;
1973 {
1974         TAILQ_REMOVE(&ip4f_list, fp, ip4f_chain);
1975         fp->ip4f_valid = 0;
1976         TAILQ_INSERT_TAIL(&ip4f_list, fp, ip4f_chain);
1977 }
1978
1979 #endif /* ALTQ3_CLFIER_COMPAT */