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