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(struct mbuf *m, struct altq_pktattr *pktattr, u_int8_t dsfield)
 816 {
 817         struct mbuf *m0;
 818
 819         if (pktattr == NULL ||
 820             (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6))
 821                 return;
 822
 823         /* verify that pattr_hdr is within the mbuf data */
 824         for (m0 = m; m0 != NULL; m0 = m0->m_next)
 825                 if ((pktattr->pattr_hdr >= m0->m_data) &&
 826                     (pktattr->pattr_hdr < m0->m_data + m0->m_len))
 827                         break;
 828         if (m0 == NULL) {
 829                 /* ick, pattr_hdr is stale */
 830                 pktattr->pattr_af = AF_UNSPEC;
 831 #ifdef ALTQ_DEBUG
 832                 printf("write_dsfield: can't locate header!\n");
 833 #endif
 834                 return;
 835         }
 836
 837         if (pktattr->pattr_af == AF_INET) {
 838                 struct ip *ip = (struct ip *)pktattr->pattr_hdr;
 839                 u_int8_t old;
 840                 int32_t sum;
 841
 842                 if (ip->ip_v != 4)
 843                         return;         /* version mismatch! */
 844                 old = ip->ip_tos;
 845                 dsfield |= old & 3;     /* leave CU bits */
 846                 if (old == dsfield)
 847                         return;
 848                 ip->ip_tos = dsfield;
 849                 /*
 850                  * update checksum (from RFC1624)
 851                  *         HC' = ~(~HC + ~m + m')
 852                  */
 853                 sum = ~ntohs(ip->ip_sum) & 0xffff;
 854                 sum += 0xff00 + (~old & 0xff) + dsfield;
 855                 sum = (sum >> 16) + (sum & 0xffff);
 856                 sum += (sum >> 16);  /* add carry */
 857
 858                 ip->ip_sum = htons(~sum & 0xffff);
 859         }
 860 #ifdef INET6
 861         else if (pktattr->pattr_af == AF_INET6) {
 862                 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr;
 863                 u_int32_t flowlabel;
 864
 865                 flowlabel = ntohl(ip6->ip6_flow);
 866                 if ((flowlabel >> 28) != 6)
 867                         return;         /* version mismatch! */
 868                 flowlabel = (flowlabel & 0xf03fffff) | (dsfield << 20);
 869                 ip6->ip6_flow = htonl(flowlabel);
 870         }
 871 #endif
 872         return;
 873 }
 874
 875
 876 /*
 877  * high resolution clock support taking advantage of a machine dependent
 878  * high resolution time counter (e.g., timestamp counter of intel pentium).
 879  * we assume
 880  *  - 64-bit-long monotonically-increasing counter
 881  *  - frequency range is 100M-4GHz (CPU speed)
 882  */
 883 /* if pcc is not available or disabled, emulate 256MHz using microtime() */
 884 #define MACHCLK_SHIFT   8
 885
 886 int machclk_usepcc;
 887 u_int32_t machclk_freq;
 888 u_int32_t machclk_per_tick;
 889
 890 #ifdef __alpha__
 891 #ifdef __FreeBSD__
 892 extern u_int32_t cycles_per_sec;        /* alpha cpu clock frequency */
 893 #elif defined(__NetBSD__) || defined(__OpenBSD__)
 894 extern u_int64_t cycles_per_usec;       /* alpha cpu clock frequency */
 895 #endif
 896 #endif /* __alpha__ */
 897 #if defined(__i386__) && defined(__NetBSD__)
 898 extern u_int64_t cpu_tsc_freq;
 899 #endif /* __alpha__ */
 900
 901 #if (__FreeBSD_version >= 700035)
 902 /* Update TSC freq with the value indicated by the caller. */
 903 static void
 904 tsc_freq_changed(void *arg, const struct cf_level *level, int status)
 905 {
 906         /* If there was an error during the transition, don't do anything. */
 907         if (status != 0)
 908                 return;
 909
 910 #if (__FreeBSD_version >= 701102) && (defined(__amd64__) || defined(__i386__))
 911         /* If TSC is P-state invariant, don't do anything. */
 912         if (tsc_is_invariant)
 913                 return;
 914 #endif
 915
 916         /* Total setting for this level gives the new frequency in MHz. */
 917         init_machclk();
 918 }
 919 EVENTHANDLER_DEFINE(cpufreq_post_change, tsc_freq_changed, NULL,
 920     EVENTHANDLER_PRI_LAST);
 921 #endif /* __FreeBSD_version >= 700035 */
 922
 923 static void
 924 init_machclk_setup(void)
 925 {
 926 #if (__FreeBSD_version >= 600000)
 927         callout_init(&tbr_callout, 0);
 928 #endif
 929
 930         machclk_usepcc = 1;
 931
 932 #if (!defined(__i386__) && !defined(__alpha__)) || defined(ALTQ_NOPCC)
 933         machclk_usepcc = 0;
 934 #endif
 935 #if defined(__FreeBSD__) && defined(SMP)
 936         machclk_usepcc = 0;
 937 #endif
 938 #if defined(__NetBSD__) && defined(MULTIPROCESSOR)
 939         machclk_usepcc = 0;
 940 #endif
 941 #ifdef __i386__
 942         /* check if TSC is available */
 943         if (machclk_usepcc == 1 && ((cpu_feature & CPUID_TSC) == 0 ||
 944             tsc_is_broken))
 945                 machclk_usepcc = 0;
 946 #endif
 947 }
 948
 949 void
 950 init_machclk(void)
 951 {
 952         static int called;
 953
 954         /* Call one-time initialization function. */
 955         if (!called) {
 956                 init_machclk_setup();
 957                 called = 1;
 958         }
 959
 960         if (machclk_usepcc == 0) {
 961                 /* emulate 256MHz using microtime() */
 962                 machclk_freq = 1000000 << MACHCLK_SHIFT;
 963                 machclk_per_tick = machclk_freq / hz;
 964 #ifdef ALTQ_DEBUG
 965                 printf("altq: emulate %uHz cpu clock\n", machclk_freq);
 966 #endif
 967                 return;
 968         }
 969
 970         /*
 971          * if the clock frequency (of Pentium TSC or Alpha PCC) is
 972          * accessible, just use it.
 973          */
 974 #ifdef __i386__
 975 #ifdef __FreeBSD__
 976 #if (__FreeBSD_version > 300000)
 977         machclk_freq = tsc_freq;
 978 #else
 979         machclk_freq = i586_ctr_freq;
 980 #endif
 981 #elif defined(__NetBSD__)
 982         machclk_freq = (u_int32_t)cpu_tsc_freq;
 983 #elif defined(__OpenBSD__) && (defined(I586_CPU) || defined(I686_CPU))
 984         machclk_freq = pentium_mhz * 1000000;
 985 #endif
 986 #elif defined(__alpha__)
 987 #ifdef __FreeBSD__
 988         machclk_freq = cycles_per_sec;
 989 #elif defined(__NetBSD__) || defined(__OpenBSD__)
 990         machclk_freq = (u_int32_t)(cycles_per_usec * 1000000);
 991 #endif
 992 #endif /* __alpha__ */
 993
 994         /*
 995          * if we don't know the clock frequency, measure it.
 996          */
 997         if (machclk_freq == 0) {
 998                 static int      wait;
 999                 struct timeval  tv_start, tv_end;
1000                 u_int64_t       start, end, diff;
1001                 int             timo;
1002
1003                 microtime(&tv_start);
1004                 start = read_machclk();
1005                 timo = hz;      /* 1 sec */
1006                 (void)tsleep(&wait, PWAIT | PCATCH, "init_machclk", timo);
1007                 microtime(&tv_end);
1008                 end = read_machclk();
1009                 diff = (u_int64_t)(tv_end.tv_sec - tv_start.tv_sec) * 1000000
1010                     + tv_end.tv_usec - tv_start.tv_usec;
1011                 if (diff != 0)
1012                         machclk_freq = (u_int)((end - start) * 1000000 / diff);
1013         }
1014
1015         machclk_per_tick = machclk_freq / hz;
1016
1017 #ifdef ALTQ_DEBUG
1018         printf("altq: CPU clock: %uHz\n", machclk_freq);
1019 #endif
1020 }
1021
1022 #if defined(__OpenBSD__) && defined(__i386__)
1023 static __inline u_int64_t
1024 rdtsc(void)
1025 {
1026         u_int64_t rv;
1027         __asm __volatile(".byte 0x0f, 0x31" : "=A" (rv));
1028         return (rv);
1029 }
1030 #endif /* __OpenBSD__ && __i386__ */
1031
1032 u_int64_t
1033 read_machclk(void)
1034 {
1035         u_int64_t val;
1036
1037         if (machclk_usepcc) {
1038 #if defined(__i386__)
1039                 val = rdtsc();
1040 #elif defined(__alpha__)
1041                 static u_int32_t last_pcc, upper;
1042                 u_int32_t pcc;
1043
1044                 /*
1045                  * for alpha, make a 64bit counter value out of the 32bit
1046                  * alpha processor cycle counter.
1047                  * read_machclk must be called within a half of its
1048                  * wrap-around cycle (about 5 sec for 400MHz cpu) to properly
1049                  * detect a counter wrap-around.
1050                  * tbr_timeout calls read_machclk once a second.
1051                  */
1052                 pcc = (u_int32_t)alpha_rpcc();
1053                 if (pcc <= last_pcc)
1054                         upper++;
1055                 last_pcc = pcc;
1056                 val = ((u_int64_t)upper << 32) + pcc;
1057 #else
1058                 panic("read_machclk");
1059 #endif
1060         } else {
1061                 struct timeval tv;
1062
1063                 microtime(&tv);
1064                 val = (((u_int64_t)(tv.tv_sec - boottime.tv_sec) * 1000000
1065                     + tv.tv_usec) << MACHCLK_SHIFT);
1066         }
1067         return (val);
1068 }
1069
1070 #ifdef ALTQ3_CLFIER_COMPAT
1071
1072 #ifndef IPPROTO_ESP
1073 #define IPPROTO_ESP     50              /* encapsulating security payload */
1074 #endif
1075 #ifndef IPPROTO_AH
1076 #define IPPROTO_AH      51              /* authentication header */
1077 #endif
1078
1079 /*
1080  * extract flow information from a given packet.
1081  * filt_mask shows flowinfo fields required.
1082  * we assume the ip header is in one mbuf, and addresses and ports are
1083  * in network byte order.
1084  */
1085 int
1086 altq_extractflow(m, af, flow, filt_bmask)
1087         struct mbuf *m;
1088         int af;
1089         struct flowinfo *flow;
1090         u_int32_t       filt_bmask;
1091 {
1092
1093         switch (af) {
1094         case PF_INET: {
1095                 struct flowinfo_in *fin;
1096                 struct ip *ip;
1097
1098                 ip = mtod(m, struct ip *);
1099
1100                 if (ip->ip_v != 4)
1101                         break;
1102
1103                 fin = (struct flowinfo_in *)flow;
1104                 fin->fi_len = sizeof(struct flowinfo_in);
1105                 fin->fi_family = AF_INET;
1106
1107                 fin->fi_proto = ip->ip_p;
1108                 fin->fi_tos = ip->ip_tos;
1109
1110                 fin->fi_src.s_addr = ip->ip_src.s_addr;
1111                 fin->fi_dst.s_addr = ip->ip_dst.s_addr;
1112
1113                 if (filt_bmask & FIMB4_PORTS)
1114                         /* if port info is required, extract port numbers */
1115                         extract_ports4(m, ip, fin);
1116                 else {
1117                         fin->fi_sport = 0;
1118                         fin->fi_dport = 0;
1119                         fin->fi_gpi = 0;
1120                 }
1121                 return (1);
1122         }
1123
1124 #ifdef INET6
1125         case PF_INET6: {
1126                 struct flowinfo_in6 *fin6;
1127                 struct ip6_hdr *ip6;
1128
1129                 ip6 = mtod(m, struct ip6_hdr *);
1130                 /* should we check the ip version? */
1131
1132                 fin6 = (struct flowinfo_in6 *)flow;
1133                 fin6->fi6_len = sizeof(struct flowinfo_in6);
1134                 fin6->fi6_family = AF_INET6;
1135
1136                 fin6->fi6_proto = ip6->ip6_nxt;
1137                 fin6->fi6_tclass   = (ntohl(ip6->ip6_flow) >> 20) & 0xff;
1138
1139                 fin6->fi6_flowlabel = ip6->ip6_flow & htonl(0x000fffff);
1140                 fin6->fi6_src = ip6->ip6_src;
1141                 fin6->fi6_dst = ip6->ip6_dst;
1142
1143                 if ((filt_bmask & FIMB6_PORTS) ||
1144                     ((filt_bmask & FIMB6_PROTO)
1145                      && ip6->ip6_nxt > IPPROTO_IPV6))
1146                         /*
1147                          * if port info is required, or proto is required
1148                          * but there are option headers, extract port
1149                          * and protocol numbers.
1150                          */
1151                         extract_ports6(m, ip6, fin6);
1152                 else {
1153                         fin6->fi6_sport = 0;
1154                         fin6->fi6_dport = 0;
1155                         fin6->fi6_gpi = 0;
1156                 }
1157                 return (1);
1158         }
1159 #endif /* INET6 */
1160
1161         default:
1162                 break;
1163         }
1164
1165         /* failed */
1166         flow->fi_len = sizeof(struct flowinfo);
1167         flow->fi_family = AF_UNSPEC;
1168         return (0);
1169 }
1170
1171 /*
1172  * helper routine to extract port numbers
1173  */
1174 /* structure for ipsec and ipv6 option header template */
1175 struct _opt6 {
1176         u_int8_t        opt6_nxt;       /* next header */
1177         u_int8_t        opt6_hlen;      /* header extension length */
1178         u_int16_t       _pad;
1179         u_int32_t       ah_spi;         /* security parameter index
1180                                            for authentication header */
1181 };
1182
1183 /*
1184  * extract port numbers from a ipv4 packet.
1185  */
1186 static int
1187 extract_ports4(m, ip, fin)
1188         struct mbuf *m;
1189         struct ip *ip;
1190         struct flowinfo_in *fin;
1191 {
1192         struct mbuf *m0;
1193         u_short ip_off;
1194         u_int8_t proto;
1195         int     off;
1196
1197         fin->fi_sport = 0;
1198         fin->fi_dport = 0;
1199         fin->fi_gpi = 0;
1200
1201         ip_off = ntohs(ip->ip_off);
1202         /* if it is a fragment, try cached fragment info */
1203         if (ip_off & IP_OFFMASK) {
1204                 ip4f_lookup(ip, fin);
1205                 return (1);
1206         }
1207
1208         /* locate the mbuf containing the protocol header */
1209         for (m0 = m; m0 != NULL; m0 = m0->m_next)
1210                 if (((caddr_t)ip >= m0->m_data) &&
1211                     ((caddr_t)ip < m0->m_data + m0->m_len))
1212                         break;
1213         if (m0 == NULL) {
1214 #ifdef ALTQ_DEBUG
1215                 printf("extract_ports4: can't locate header! ip=%p\n", ip);
1216 #endif
1217                 return (0);
1218         }
1219         off = ((caddr_t)ip - m0->m_data) + (ip->ip_hl << 2);
1220         proto = ip->ip_p;
1221
1222 #ifdef ALTQ_IPSEC
1223  again:
1224 #endif
1225         while (off >= m0->m_len) {
1226                 off -= m0->m_len;
1227                 m0 = m0->m_next;
1228                 if (m0 == NULL)
1229                         return (0);  /* bogus ip_hl! */
1230         }
1231         if (m0->m_len < off + 4)
1232                 return (0);
1233
1234         switch (proto) {
1235         case IPPROTO_TCP:
1236         case IPPROTO_UDP: {
1237                 struct udphdr *udp;
1238
1239                 udp = (struct udphdr *)(mtod(m0, caddr_t) + off);
1240                 fin->fi_sport = udp->uh_sport;
1241                 fin->fi_dport = udp->uh_dport;
1242                 fin->fi_proto = proto;
1243                 }
1244                 break;
1245
1246 #ifdef ALTQ_IPSEC
1247         case IPPROTO_ESP:
1248                 if (fin->fi_gpi == 0){
1249                         u_int32_t *gpi;
1250
1251                         gpi = (u_int32_t *)(mtod(m0, caddr_t) + off);
1252                         fin->fi_gpi   = *gpi;
1253                 }
1254                 fin->fi_proto = proto;
1255                 break;
1256
1257         case IPPROTO_AH: {
1258                         /* get next header and header length */
1259                         struct _opt6 *opt6;
1260
1261                         opt6 = (struct _opt6 *)(mtod(m0, caddr_t) + off);
1262                         proto = opt6->opt6_nxt;
1263                         off += 8 + (opt6->opt6_hlen * 4);
1264                         if (fin->fi_gpi == 0 && m0->m_len >= off + 8)
1265                                 fin->fi_gpi = opt6->ah_spi;
1266                 }
1267                 /* goto the next header */
1268                 goto again;
1269 #endif  /* ALTQ_IPSEC */
1270
1271         default:
1272                 fin->fi_proto = proto;
1273                 return (0);
1274         }
1275
1276         /* if this is a first fragment, cache it. */
1277         if (ip_off & IP_MF)
1278                 ip4f_cache(ip, fin);
1279
1280         return (1);
1281 }
1282
1283 #ifdef INET6
1284 static int
1285 extract_ports6(m, ip6, fin6)
1286         struct mbuf *m;
1287         struct ip6_hdr *ip6;
1288         struct flowinfo_in6 *fin6;
1289 {
1290         struct mbuf *m0;
1291         int     off;
1292         u_int8_t proto;
1293
1294         fin6->fi6_gpi   = 0;
1295         fin6->fi6_sport = 0;
1296         fin6->fi6_dport = 0;
1297
1298         /* locate the mbuf containing the protocol header */
1299         for (m0 = m; m0 != NULL; m0 = m0->m_next)
1300                 if (((caddr_t)ip6 >= m0->m_data) &&
1301                     ((caddr_t)ip6 < m0->m_data + m0->m_len))
1302                         break;
1303         if (m0 == NULL) {
1304 #ifdef ALTQ_DEBUG
1305                 printf("extract_ports6: can't locate header! ip6=%p\n", ip6);
1306 #endif
1307                 return (0);
1308         }
1309         off = ((caddr_t)ip6 - m0->m_data) + sizeof(struct ip6_hdr);
1310
1311         proto = ip6->ip6_nxt;
1312         do {
1313                 while (off >= m0->m_len) {
1314                         off -= m0->m_len;
1315                         m0 = m0->m_next;
1316                         if (m0 == NULL)
1317                                 return (0);
1318                 }
1319                 if (m0->m_len < off + 4)
1320                         return (0);
1321
1322                 switch (proto) {
1323                 case IPPROTO_TCP:
1324                 case IPPROTO_UDP: {
1325                         struct udphdr *udp;
1326
1327                         udp = (struct udphdr *)(mtod(m0, caddr_t) + off);
1328                         fin6->fi6_sport = udp->uh_sport;
1329                         fin6->fi6_dport = udp->uh_dport;
1330                         fin6->fi6_proto = proto;
1331                         }
1332                         return (1);
1333
1334                 case IPPROTO_ESP:
1335                         if (fin6->fi6_gpi == 0) {
1336                                 u_int32_t *gpi;
1337
1338                                 gpi = (u_int32_t *)(mtod(m0, caddr_t) + off);
1339                                 fin6->fi6_gpi   = *gpi;
1340                         }
1341                         fin6->fi6_proto = proto;
1342                         return (1);
1343
1344                 case IPPROTO_AH: {
1345                         /* get next header and header length */
1346                         struct _opt6 *opt6;
1347
1348                         opt6 = (struct _opt6 *)(mtod(m0, caddr_t) + off);
1349                         if (fin6->fi6_gpi == 0 && m0->m_len >= off + 8)
1350                                 fin6->fi6_gpi = opt6->ah_spi;
1351                         proto = opt6->opt6_nxt;
1352                         off += 8 + (opt6->opt6_hlen * 4);
1353                         /* goto the next header */
1354                         break;
1355                         }
1356
1357                 case IPPROTO_HOPOPTS:
1358                 case IPPROTO_ROUTING:
1359                 case IPPROTO_DSTOPTS: {
1360                         /* get next header and header length */
1361                         struct _opt6 *opt6;
1362
1363                         opt6 = (struct _opt6 *)(mtod(m0, caddr_t) + off);
1364                         proto = opt6->opt6_nxt;
1365                         off += (opt6->opt6_hlen + 1) * 8;
1366                         /* goto the next header */
1367                         break;
1368                         }
1369
1370                 case IPPROTO_FRAGMENT:
1371                         /* ipv6 fragmentations are not supported yet */
1372                 default:
1373                         fin6->fi6_proto = proto;
1374                         return (0);
1375                 }
1376         } while (1);
1377         /*NOTREACHED*/
1378 }
1379 #endif /* INET6 */
1380
1381 /*
1382  * altq common classifier
1383  */
1384 int
1385 acc_add_filter(classifier, filter, class, phandle)
1386         struct acc_classifier *classifier;
1387         struct flow_filter *filter;
1388         void    *class;
1389         u_long  *phandle;
1390 {
1391         struct acc_filter *afp, *prev, *tmp;
1392         int     i, s;
1393
1394 #ifdef INET6
1395         if (filter->ff_flow.fi_family != AF_INET &&
1396             filter->ff_flow.fi_family != AF_INET6)
1397                 return (EINVAL);
1398 #else
1399         if (filter->ff_flow.fi_family != AF_INET)
1400                 return (EINVAL);
1401 #endif
1402
1403         MALLOC(afp, struct acc_filter *, sizeof(struct acc_filter),
1404                M_DEVBUF, M_WAITOK);
1405         if (afp == NULL)
1406                 return (ENOMEM);
1407         bzero(afp, sizeof(struct acc_filter));
1408
1409         afp->f_filter = *filter;
1410         afp->f_class = class;
1411
1412         i = ACC_WILDCARD_INDEX;
1413         if (filter->ff_flow.fi_family == AF_INET) {
1414                 struct flow_filter *filter4 = &afp->f_filter;
1415
1416                 /*
1417                  * if address is 0, it's a wildcard.  if address mask
1418                  * isn't set, use full mask.
1419                  */
1420                 if (filter4->ff_flow.fi_dst.s_addr == 0)
1421                         filter4->ff_mask.mask_dst.s_addr = 0;
1422                 else if (filter4->ff_mask.mask_dst.s_addr == 0)
1423                         filter4->ff_mask.mask_dst.s_addr = 0xffffffff;
1424                 if (filter4->ff_flow.fi_src.s_addr == 0)
1425                         filter4->ff_mask.mask_src.s_addr = 0;
1426                 else if (filter4->ff_mask.mask_src.s_addr == 0)
1427                         filter4->ff_mask.mask_src.s_addr = 0xffffffff;
1428
1429                 /* clear extra bits in addresses  */
1430                    filter4->ff_flow.fi_dst.s_addr &=
1431                        filter4->ff_mask.mask_dst.s_addr;
1432                    filter4->ff_flow.fi_src.s_addr &=
1433                        filter4->ff_mask.mask_src.s_addr;
1434
1435                 /*
1436                  * if dst address is a wildcard, use hash-entry
1437                  * ACC_WILDCARD_INDEX.
1438                  */
1439                 if (filter4->ff_mask.mask_dst.s_addr != 0xffffffff)
1440                         i = ACC_WILDCARD_INDEX;
1441                 else
1442                         i = ACC_GET_HASH_INDEX(filter4->ff_flow.fi_dst.s_addr);
1443         }
1444 #ifdef INET6
1445         else if (filter->ff_flow.fi_family == AF_INET6) {
1446                 struct flow_filter6 *filter6 =
1447                         (struct flow_filter6 *)&afp->f_filter;
1448 #ifndef IN6MASK0 /* taken from kame ipv6 */
1449 #define IN6MASK0        {{{ 0, 0, 0, 0 }}}
1450 #define IN6MASK128      {{{ 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff }}}
1451                 const struct in6_addr in6mask0 = IN6MASK0;
1452                 const struct in6_addr in6mask128 = IN6MASK128;
1453 #endif
1454
1455                 if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_flow6.fi6_dst))
1456                         filter6->ff_mask6.mask6_dst = in6mask0;
1457                 else if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_mask6.mask6_dst))
1458                         filter6->ff_mask6.mask6_dst = in6mask128;
1459                 if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_flow6.fi6_src))
1460                         filter6->ff_mask6.mask6_src = in6mask0;
1461                 else if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_mask6.mask6_src))
1462                         filter6->ff_mask6.mask6_src = in6mask128;
1463
1464                 /* clear extra bits in addresses  */
1465                 for (i = 0; i < 16; i++)
1466                         filter6->ff_flow6.fi6_dst.s6_addr[i] &=
1467                             filter6->ff_mask6.mask6_dst.s6_addr[i];
1468                 for (i = 0; i < 16; i++)
1469                         filter6->ff_flow6.fi6_src.s6_addr[i] &=
1470                             filter6->ff_mask6.mask6_src.s6_addr[i];
1471
1472                 if (filter6->ff_flow6.fi6_flowlabel == 0)
1473                         i = ACC_WILDCARD_INDEX;
1474                 else
1475                         i = ACC_GET_HASH_INDEX(filter6->ff_flow6.fi6_flowlabel);
1476         }
1477 #endif /* INET6 */
1478
1479         afp->f_handle = get_filt_handle(classifier, i);
1480
1481         /* update filter bitmask */
1482         afp->f_fbmask = filt2fibmask(filter);
1483         classifier->acc_fbmask |= afp->f_fbmask;
1484
1485         /*
1486          * add this filter to the filter list.
1487          * filters are ordered from the highest rule number.
1488          */
1489 #ifdef __NetBSD__
1490         s = splnet();
1491 #else
1492         s = splimp();
1493 #endif
1494         prev = NULL;
1495         LIST_FOREACH(tmp, &classifier->acc_filters[i], f_chain) {
1496                 if (tmp->f_filter.ff_ruleno > afp->f_filter.ff_ruleno)
1497                         prev = tmp;
1498                 else
1499                         break;
1500         }
1501         if (prev == NULL)
1502                 LIST_INSERT_HEAD(&classifier->acc_filters[i], afp, f_chain);
1503         else
1504                 LIST_INSERT_AFTER(prev, afp, f_chain);
1505         splx(s);
1506
1507         *phandle = afp->f_handle;
1508         return (0);
1509 }
1510
1511 int
1512 acc_delete_filter(classifier, handle)
1513         struct acc_classifier *classifier;
1514         u_long handle;
1515 {
1516         struct acc_filter *afp;
1517         int     s;
1518
1519         if ((afp = filth_to_filtp(classifier, handle)) == NULL)
1520                 return (EINVAL);
1521
1522 #ifdef __NetBSD__
1523         s = splnet();
1524 #else
1525         s = splimp();
1526 #endif
1527         LIST_REMOVE(afp, f_chain);
1528         splx(s);
1529
1530         FREE(afp, M_DEVBUF);
1531
1532         /* todo: update filt_bmask */
1533
1534         return (0);
1535 }
1536
1537 /*
1538  * delete filters referencing to the specified class.
1539  * if the all flag is not 0, delete all the filters.
1540  */
1541 int
1542 acc_discard_filters(classifier, class, all)
1543         struct acc_classifier *classifier;
1544         void    *class;
1545         int     all;
1546 {
1547         struct acc_filter *afp;
1548         int     i, s;
1549
1550 #ifdef __NetBSD__
1551         s = splnet();
1552 #else
1553         s = splimp();
1554 #endif
1555         for (i = 0; i < ACC_FILTER_TABLESIZE; i++) {
1556                 do {
1557                         LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
1558                                 if (all || afp->f_class == class) {
1559                                         LIST_REMOVE(afp, f_chain);
1560                                         FREE(afp, M_DEVBUF);
1561                                         /* start again from the head */
1562                                         break;
1563                                 }
1564                 } while (afp != NULL);
1565         }
1566         splx(s);
1567
1568         if (all)
1569                 classifier->acc_fbmask = 0;
1570
1571         return (0);
1572 }
1573
1574 void *
1575 acc_classify(clfier, m, af)
1576         void *clfier;
1577         struct mbuf *m;
1578         int af;
1579 {
1580         struct acc_classifier *classifier;
1581         struct flowinfo flow;
1582         struct acc_filter *afp;
1583         int     i;
1584
1585         classifier = (struct acc_classifier *)clfier;
1586         altq_extractflow(m, af, &flow, classifier->acc_fbmask);
1587
1588         if (flow.fi_family == AF_INET) {
1589                 struct flowinfo_in *fp = (struct flowinfo_in *)&flow;
1590
1591                 if ((classifier->acc_fbmask & FIMB4_ALL) == FIMB4_TOS) {
1592                         /* only tos is used */
1593                         LIST_FOREACH(afp,
1594                                  &classifier->acc_filters[ACC_WILDCARD_INDEX],
1595                                  f_chain)
1596                                 if (apply_tosfilter4(afp->f_fbmask,
1597                                                      &afp->f_filter, fp))
1598                                         /* filter matched */
1599                                         return (afp->f_class);
1600                 } else if ((classifier->acc_fbmask &
1601                         (~(FIMB4_PROTO|FIMB4_SPORT|FIMB4_DPORT) & FIMB4_ALL))
1602                     == 0) {
1603                         /* only proto and ports are used */
1604                         LIST_FOREACH(afp,
1605                                  &classifier->acc_filters[ACC_WILDCARD_INDEX],
1606                                  f_chain)
1607                                 if (apply_ppfilter4(afp->f_fbmask,
1608                                                     &afp->f_filter, fp))
1609                                         /* filter matched */
1610                                         return (afp->f_class);
1611                 } else {
1612                         /* get the filter hash entry from its dest address */
1613                         i = ACC_GET_HASH_INDEX(fp->fi_dst.s_addr);
1614                         do {
1615                                 /*
1616                                  * go through this loop twice.  first for dst
1617                                  * hash, second for wildcards.
1618                                  */
1619                                 LIST_FOREACH(afp, &classifier->acc_filters[i],
1620                                              f_chain)
1621                                         if (apply_filter4(afp->f_fbmask,
1622                                                           &afp->f_filter, fp))
1623                                                 /* filter matched */
1624                                                 return (afp->f_class);
1625
1626                                 /*
1627                                  * check again for filters with a dst addr
1628                                  * wildcard.
1629                                  * (daddr == 0 || dmask != 0xffffffff).
1630                                  */
1631                                 if (i != ACC_WILDCARD_INDEX)
1632                                         i = ACC_WILDCARD_INDEX;
1633                                 else
1634                                         break;
1635                         } while (1);
1636                 }
1637         }
1638 #ifdef INET6
1639         else if (flow.fi_family == AF_INET6) {
1640                 struct flowinfo_in6 *fp6 = (struct flowinfo_in6 *)&flow;
1641
1642                 /* get the filter hash entry from its flow ID */
1643                 if (fp6->fi6_flowlabel != 0)
1644                         i = ACC_GET_HASH_INDEX(fp6->fi6_flowlabel);
1645                 else
1646                         /* flowlable can be zero */
1647                         i = ACC_WILDCARD_INDEX;
1648
1649                 /* go through this loop twice.  first for flow hash, second
1650                    for wildcards. */
1651                 do {
1652                         LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
1653                                 if (apply_filter6(afp->f_fbmask,
1654                                         (struct flow_filter6 *)&afp->f_filter,
1655                                         fp6))
1656                                         /* filter matched */
1657                                         return (afp->f_class);
1658
1659                         /*
1660                          * check again for filters with a wildcard.
1661                          */
1662                         if (i != ACC_WILDCARD_INDEX)
1663                                 i = ACC_WILDCARD_INDEX;
1664                         else
1665                                 break;
1666                 } while (1);
1667         }
1668 #endif /* INET6 */
1669
1670         /* no filter matched */
1671         return (NULL);
1672 }
1673
1674 static int
1675 apply_filter4(fbmask, filt, pkt)
1676         u_int32_t       fbmask;
1677         struct flow_filter *filt;
1678         struct flowinfo_in *pkt;
1679 {
1680         if (filt->ff_flow.fi_family != AF_INET)
1681                 return (0);
1682         if ((fbmask & FIMB4_SPORT) && filt->ff_flow.fi_sport != pkt->fi_sport)
1683                 return (0);
1684         if ((fbmask & FIMB4_DPORT) && filt->ff_flow.fi_dport != pkt->fi_dport)
1685                 return (0);
1686         if ((fbmask & FIMB4_DADDR) &&
1687             filt->ff_flow.fi_dst.s_addr !=
1688             (pkt->fi_dst.s_addr & filt->ff_mask.mask_dst.s_addr))
1689                 return (0);
1690         if ((fbmask & FIMB4_SADDR) &&
1691             filt->ff_flow.fi_src.s_addr !=
1692             (pkt->fi_src.s_addr & filt->ff_mask.mask_src.s_addr))
1693                 return (0);
1694         if ((fbmask & FIMB4_PROTO) && filt->ff_flow.fi_proto != pkt->fi_proto)
1695                 return (0);
1696         if ((fbmask & FIMB4_TOS) && filt->ff_flow.fi_tos !=
1697             (pkt->fi_tos & filt->ff_mask.mask_tos))
1698                 return (0);
1699         if ((fbmask & FIMB4_GPI) && filt->ff_flow.fi_gpi != (pkt->fi_gpi))
1700                 return (0);
1701         /* match */
1702         return (1);
1703 }
1704
1705 /*
1706  * filter matching function optimized for a common case that checks
1707  * only protocol and port numbers
1708  */
1709 static int
1710 apply_ppfilter4(fbmask, filt, pkt)
1711         u_int32_t       fbmask;
1712         struct flow_filter *filt;
1713         struct flowinfo_in *pkt;
1714 {
1715         if (filt->ff_flow.fi_family != AF_INET)
1716                 return (0);
1717         if ((fbmask & FIMB4_SPORT) && filt->ff_flow.fi_sport != pkt->fi_sport)
1718                 return (0);
1719         if ((fbmask & FIMB4_DPORT) && filt->ff_flow.fi_dport != pkt->fi_dport)
1720                 return (0);
1721         if ((fbmask & FIMB4_PROTO) && filt->ff_flow.fi_proto != pkt->fi_proto)
1722                 return (0);
1723         /* match */
1724         return (1);
1725 }
1726
1727 /*
1728  * filter matching function only for tos field.
1729  */
1730 static int
1731 apply_tosfilter4(fbmask, filt, pkt)
1732         u_int32_t       fbmask;
1733         struct flow_filter *filt;
1734         struct flowinfo_in *pkt;
1735 {
1736         if (filt->ff_flow.fi_family != AF_INET)
1737                 return (0);
1738         if ((fbmask & FIMB4_TOS) && filt->ff_flow.fi_tos !=
1739             (pkt->fi_tos & filt->ff_mask.mask_tos))
1740                 return (0);
1741         /* match */
1742         return (1);
1743 }
1744
1745 #ifdef INET6
1746 static int
1747 apply_filter6(fbmask, filt, pkt)
1748         u_int32_t       fbmask;
1749         struct flow_filter6 *filt;
1750         struct flowinfo_in6 *pkt;
1751 {
1752         int i;
1753
1754         if (filt->ff_flow6.fi6_family != AF_INET6)
1755                 return (0);
1756         if ((fbmask & FIMB6_FLABEL) &&
1757             filt->ff_flow6.fi6_flowlabel != pkt->fi6_flowlabel)
1758                 return (0);
1759         if ((fbmask & FIMB6_PROTO) &&
1760             filt->ff_flow6.fi6_proto != pkt->fi6_proto)
1761                 return (0);
1762         if ((fbmask & FIMB6_SPORT) &&
1763             filt->ff_flow6.fi6_sport != pkt->fi6_sport)
1764                 return (0);
1765         if ((fbmask & FIMB6_DPORT) &&
1766             filt->ff_flow6.fi6_dport != pkt->fi6_dport)
1767                 return (0);
1768         if (fbmask & FIMB6_SADDR) {
1769                 for (i = 0; i < 4; i++)
1770                         if (filt->ff_flow6.fi6_src.s6_addr32[i] !=
1771                             (pkt->fi6_src.s6_addr32[i] &
1772                              filt->ff_mask6.mask6_src.s6_addr32[i]))
1773                                 return (0);
1774         }
1775         if (fbmask & FIMB6_DADDR) {
1776                 for (i = 0; i < 4; i++)
1777                         if (filt->ff_flow6.fi6_dst.s6_addr32[i] !=
1778                             (pkt->fi6_dst.s6_addr32[i] &
1779                              filt->ff_mask6.mask6_dst.s6_addr32[i]))
1780                                 return (0);
1781         }
1782         if ((fbmask & FIMB6_TCLASS) &&
1783             filt->ff_flow6.fi6_tclass !=
1784             (pkt->fi6_tclass & filt->ff_mask6.mask6_tclass))
1785                 return (0);
1786         if ((fbmask & FIMB6_GPI) &&
1787             filt->ff_flow6.fi6_gpi != pkt->fi6_gpi)
1788                 return (0);
1789         /* match */
1790         return (1);
1791 }
1792 #endif /* INET6 */
1793
1794 /*
1795  *  filter handle:
1796  *      bit 20-28: index to the filter hash table
1797  *      bit  0-19: unique id in the hash bucket.
1798  */
1799 static u_long
1800 get_filt_handle(classifier, i)
1801         struct acc_classifier *classifier;
1802         int     i;
1803 {
1804         static u_long handle_number = 1;
1805         u_long  handle;
1806         struct acc_filter *afp;
1807
1808         while (1) {
1809                 handle = handle_number++ & 0x000fffff;
1810
1811                 if (LIST_EMPTY(&classifier->acc_filters[i]))
1812                         break;
1813
1814                 LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
1815                         if ((afp->f_handle & 0x000fffff) == handle)
1816                                 break;
1817                 if (afp == NULL)
1818                         break;
1819                 /* this handle is already used, try again */
1820         }
1821
1822         return ((i << 20) | handle);
1823 }
1824
1825 /* convert filter handle to filter pointer */
1826 static struct acc_filter *
1827 filth_to_filtp(classifier, handle)
1828         struct acc_classifier *classifier;
1829         u_long handle;
1830 {
1831         struct acc_filter *afp;
1832         int     i;
1833
1834         i = ACC_GET_HINDEX(handle);
1835
1836         LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
1837                 if (afp->f_handle == handle)
1838                         return (afp);
1839
1840         return (NULL);
1841 }
1842
1843 /* create flowinfo bitmask */
1844 static u_int32_t
1845 filt2fibmask(filt)
1846         struct flow_filter *filt;
1847 {
1848         u_int32_t mask = 0;
1849 #ifdef INET6
1850         struct flow_filter6 *filt6;
1851 #endif
1852
1853         switch (filt->ff_flow.fi_family) {
1854         case AF_INET:
1855                 if (filt->ff_flow.fi_proto != 0)
1856                         mask |= FIMB4_PROTO;
1857                 if (filt->ff_flow.fi_tos != 0)
1858                         mask |= FIMB4_TOS;
1859                 if (filt->ff_flow.fi_dst.s_addr != 0)
1860                         mask |= FIMB4_DADDR;
1861                 if (filt->ff_flow.fi_src.s_addr != 0)
1862                         mask |= FIMB4_SADDR;
1863                 if (filt->ff_flow.fi_sport != 0)
1864                         mask |= FIMB4_SPORT;
1865                 if (filt->ff_flow.fi_dport != 0)
1866                         mask |= FIMB4_DPORT;
1867                 if (filt->ff_flow.fi_gpi != 0)
1868                         mask |= FIMB4_GPI;
1869                 break;
1870 #ifdef INET6
1871         case AF_INET6:
1872                 filt6 = (struct flow_filter6 *)filt;
1873
1874                 if (filt6->ff_flow6.fi6_proto != 0)
1875                         mask |= FIMB6_PROTO;
1876                 if (filt6->ff_flow6.fi6_tclass != 0)
1877                         mask |= FIMB6_TCLASS;
1878                 if (!IN6_IS_ADDR_UNSPECIFIED(&filt6->ff_flow6.fi6_dst))
1879                         mask |= FIMB6_DADDR;
1880                 if (!IN6_IS_ADDR_UNSPECIFIED(&filt6->ff_flow6.fi6_src))
1881                         mask |= FIMB6_SADDR;
1882                 if (filt6->ff_flow6.fi6_sport != 0)
1883                         mask |= FIMB6_SPORT;
1884                 if (filt6->ff_flow6.fi6_dport != 0)
1885                         mask |= FIMB6_DPORT;
1886                 if (filt6->ff_flow6.fi6_gpi != 0)
1887                         mask |= FIMB6_GPI;
1888                 if (filt6->ff_flow6.fi6_flowlabel != 0)
1889                         mask |= FIMB6_FLABEL;
1890                 break;
1891 #endif /* INET6 */
1892         }
1893         return (mask);
1894 }
1895
1896
1897 /*
1898  * helper functions to handle IPv4 fragments.
1899  * currently only in-sequence fragments are handled.
1900  *      - fragment info is cached in a LRU list.
1901  *      - when a first fragment is found, cache its flow info.
1902  *      - when a non-first fragment is found, lookup the cache.
1903  */
1904
1905 struct ip4_frag {
1906     TAILQ_ENTRY(ip4_frag) ip4f_chain;
1907     char    ip4f_valid;
1908     u_short ip4f_id;
1909     struct flowinfo_in ip4f_info;
1910 };
1911
1912 static TAILQ_HEAD(ip4f_list, ip4_frag) ip4f_list; /* IPv4 fragment cache */
1913
1914 #define IP4F_TABSIZE            16      /* IPv4 fragment cache size */
1915
1916
1917 static void
1918 ip4f_cache(ip, fin)
1919         struct ip *ip;
1920         struct flowinfo_in *fin;
1921 {
1922         struct ip4_frag *fp;
1923
1924         if (TAILQ_EMPTY(&ip4f_list)) {
1925                 /* first time call, allocate fragment cache entries. */
1926                 if (ip4f_init() < 0)
1927                         /* allocation failed! */
1928                         return;
1929         }
1930
1931         fp = ip4f_alloc();
1932         fp->ip4f_id = ip->ip_id;
1933         fp->ip4f_info.fi_proto = ip->ip_p;
1934         fp->ip4f_info.fi_src.s_addr = ip->ip_src.s_addr;
1935         fp->ip4f_info.fi_dst.s_addr = ip->ip_dst.s_addr;
1936
1937         /* save port numbers */
1938         fp->ip4f_info.fi_sport = fin->fi_sport;
1939         fp->ip4f_info.fi_dport = fin->fi_dport;
1940         fp->ip4f_info.fi_gpi   = fin->fi_gpi;
1941 }
1942
1943 static int
1944 ip4f_lookup(ip, fin)
1945         struct ip *ip;
1946         struct flowinfo_in *fin;
1947 {
1948         struct ip4_frag *fp;
1949
1950         for (fp = TAILQ_FIRST(&ip4f_list); fp != NULL && fp->ip4f_valid;
1951              fp = TAILQ_NEXT(fp, ip4f_chain))
1952                 if (ip->ip_id == fp->ip4f_id &&
1953                     ip->ip_src.s_addr == fp->ip4f_info.fi_src.s_addr &&
1954                     ip->ip_dst.s_addr == fp->ip4f_info.fi_dst.s_addr &&
1955                     ip->ip_p == fp->ip4f_info.fi_proto) {
1956
1957                         /* found the matching entry */
1958                         fin->fi_sport = fp->ip4f_info.fi_sport;
1959                         fin->fi_dport = fp->ip4f_info.fi_dport;
1960                         fin->fi_gpi   = fp->ip4f_info.fi_gpi;
1961
1962                         if ((ntohs(ip->ip_off) & IP_MF) == 0)
1963                                 /* this is the last fragment,
1964                                    release the entry. */
1965                                 ip4f_free(fp);
1966
1967                         return (1);
1968                 }
1969
1970         /* no matching entry found */
1971         return (0);
1972 }
1973
1974 static int
1975 ip4f_init(void)
1976 {
1977         struct ip4_frag *fp;
1978         int i;
1979
1980         TAILQ_INIT(&ip4f_list);
1981         for (i=0; i<IP4F_TABSIZE; i++) {
1982                 MALLOC(fp, struct ip4_frag *, sizeof(struct ip4_frag),
1983                        M_DEVBUF, M_NOWAIT);
1984                 if (fp == NULL) {
1985                         printf("ip4f_init: can't alloc %dth entry!\n", i);
1986                         if (i == 0)
1987                                 return (-1);
1988                         return (0);
1989                 }
1990                 fp->ip4f_valid = 0;
1991                 TAILQ_INSERT_TAIL(&ip4f_list, fp, ip4f_chain);
1992         }
1993         return (0);
1994 }
1995
1996 static struct ip4_frag *
1997 ip4f_alloc(void)
1998 {
1999         struct ip4_frag *fp;
2000
2001         /* reclaim an entry at the tail, put it at the head */
2002         fp = TAILQ_LAST(&ip4f_list, ip4f_list);
2003         TAILQ_REMOVE(&ip4f_list, fp, ip4f_chain);
2004         fp->ip4f_valid = 1;
2005         TAILQ_INSERT_HEAD(&ip4f_list, fp, ip4f_chain);
2006         return (fp);
2007 }
2008
2009 static void
2010 ip4f_free(fp)
2011         struct ip4_frag *fp;
2012 {
2013         TAILQ_REMOVE(&ip4f_list, fp, ip4f_chain);
2014         fp->ip4f_valid = 0;
2015         TAILQ_INSERT_TAIL(&ip4f_list, fp, ip4f_chain);
2016 }
2017
2018 #endif /* ALTQ3_CLFIER_COMPAT */