sys/contrib/altq/altq/altq_subr.c

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