sys/net/altq/altq_fairq.c

   1 /*
   2  * Copyright (c) 2008 The DragonFly Project.  All rights reserved.
   3  *
   4  * This code is derived from software contributed to The DragonFly Project
   5  * by Matthew Dillon <dillon@backplane.com>
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions
   9  * are met:
  10  *
  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
  15  *    the documentation and/or other materials provided with the
  16  *    distribution.
  17  * 3. Neither the name of The DragonFly Project nor the names of its
  18  *    contributors may be used to endorse or promote products derived
  19  *    from this software without specific, prior written permission.
  20  *
  21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  22  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE
  25  * COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  26  * INCIDENTAL, SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING,
  27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
  29  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  30  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
  31  * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  32  * SUCH DAMAGE.
  33  *
  34  * $DragonFly: src/sys/net/altq/altq_fairq.c,v 1.2 2008/05/14 11:59:23 sephe Exp $
  35  */
  36 /*
  37  * Matt: I gutted altq_priq.c and used it as a skeleton on which to build
  38  * fairq.  The fairq algorithm is completely different then priq, of course,
  39  * but because I used priq's skeleton I believe I should include priq's
  40  * copyright.
  41  *
  42  * Copyright (C) 2000-2003
  43  *      Sony Computer Science Laboratories Inc.  All rights reserved.
  44  *
  45  * Redistribution and use in source and binary forms, with or without
  46  * modification, are permitted provided that the following conditions
  47  * are met:
  48  * 1. Redistributions of source code must retain the above copyright
  49  *    notice, this list of conditions and the following disclaimer.
  50  * 2. Redistributions in binary form must reproduce the above copyright
  51  *    notice, this list of conditions and the following disclaimer in the
  52  *    documentation and/or other materials provided with the distribution.
  53  *
  54  * THIS SOFTWARE IS PROVIDED BY SONY CSL AND CONTRIBUTORS ``AS IS'' AND
  55  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  56  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  57  * ARE DISCLAIMED.  IN NO EVENT SHALL SONY CSL OR CONTRIBUTORS BE LIABLE
  58  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  59  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  60  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  61  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  62  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  63  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  64  * SUCH DAMAGE.
  65  */
  66
  67 /*
  68  * FAIRQ - take traffic classified by keep state (hashed into
  69  * mbuf->m_pkthdr.altq_state_hash) and bucketize it.  Fairly extract
  70  * the first packet from each bucket in a round-robin fashion.
  71  *
  72  * TODO - better overall qlimit support (right now it is per-bucket).
  73  *      - NOTE: red etc is per bucket, not overall.
  74  *      - better service curve support.
  75  *
  76  * EXAMPLE:
  77  *
  78  *  altq on em0 fairq bandwidth 650Kb queue { std, bulk }
  79  *  queue std  priority 3 bandwidth 400Kb \
  80  *      fairq (buckets 64, default, hogs 1Kb) qlimit 50
  81  *  queue bulk priority 2 bandwidth 100Kb \
  82  *      fairq (buckets 64, hogs 1Kb) qlimit 50
  83  *
  84  *  pass out on em0 from any to any keep state queue std
  85  *  pass out on em0 inet proto tcp ..... port ... keep state queue bulk
  86  */
  87 #include "opt_altq.h"
  88 #include "opt_inet.h"
  89 #include "opt_inet6.h"
  90
  91 #ifdef ALTQ_FAIRQ  /* fairq is enabled in the kernel conf */
  92
  93 #include <sys/param.h>
  94 #include <sys/malloc.h>
  95 #include <sys/mbuf.h>
  96 #include <sys/socket.h>
  97 #include <sys/sockio.h>
  98 #include <sys/systm.h>
  99 #include <sys/proc.h>
 100 #include <sys/errno.h>
 101 #include <sys/kernel.h>
 102 #include <sys/queue.h>
 103 #include <sys/thread.h>
 104
 105 #include <net/if.h>
 106 #include <net/ifq_var.h>
 107 #include <netinet/in.h>
 108
 109 #include <net/pf/pfvar.h>
 110 #include <net/altq/altq.h>
 111 #include <net/altq/altq_fairq.h>
 112
 113 #include <sys/thread2.h>
 114
 115 /*
 116  * function prototypes
 117  */
 118 static int      fairq_clear_interface(struct fairq_if *);
 119 static int      fairq_request(struct ifaltq *, int, void *);
 120 static void     fairq_purge(struct fairq_if *);
 121 static struct fairq_class *fairq_class_create(struct fairq_if *, int, int, u_int, struct fairq_opts *, int);
 122 static int      fairq_class_destroy(struct fairq_class *);
 123 static int      fairq_enqueue(struct ifaltq *, struct mbuf *, struct altq_pktattr *);
 124 static struct mbuf *fairq_dequeue(struct ifaltq *, struct mbuf *, int);
 125
 126 static int      fairq_addq(struct fairq_class *, struct mbuf *);
 127 static struct mbuf *fairq_getq(struct fairq_class *, uint64_t);
 128 static struct mbuf *fairq_pollq(struct fairq_class *, uint64_t, int *);
 129 static fairq_bucket_t *fairq_selectq(struct fairq_class *, int);
 130 static void     fairq_purgeq(struct fairq_class *);
 131
 132 static void     get_class_stats(struct fairq_classstats *, struct fairq_class *);
 133 static struct fairq_class *clh_to_clp(struct fairq_if *, uint32_t);
 134
 135 int
 136 fairq_pfattach(struct pf_altq *a, struct ifaltq *ifq)
 137 {
 138         return altq_attach(ifq, ALTQT_FAIRQ, a->altq_disc,
 139             fairq_enqueue, fairq_dequeue, fairq_request, NULL, NULL);
 140 }
 141
 142 int
 143 fairq_add_altq(struct pf_altq *a)
 144 {
 145         struct fairq_if *pif;
 146         struct ifnet *ifp;
 147
 148         if ((ifp = ifunit(a->ifname)) == NULL)
 149                 return (EINVAL);
 150         if (!ifq_is_ready(&ifp->if_snd))
 151                 return (ENODEV);
 152
 153         pif = kmalloc(sizeof(*pif), M_ALTQ, M_WAITOK | M_ZERO);
 154         pif->pif_bandwidth = a->ifbandwidth;
 155         pif->pif_maxpri = -1;
 156         pif->pif_ifq = &ifp->if_snd;
 157         ifq_purge(&ifp->if_snd);
 158
 159         /* keep the state in pf_altq */
 160         a->altq_disc = pif;
 161
 162         return (0);
 163 }
 164
 165 int
 166 fairq_remove_altq(struct pf_altq *a)
 167 {
 168         struct fairq_if *pif;
 169
 170         if ((pif = a->altq_disc) == NULL)
 171                 return (EINVAL);
 172         a->altq_disc = NULL;
 173
 174         fairq_clear_interface(pif);
 175
 176         kfree(pif, M_ALTQ);
 177         return (0);
 178 }
 179
 180 static int
 181 fairq_add_queue_locked(struct pf_altq *a, struct fairq_if *pif)
 182 {
 183         struct fairq_class *cl;
 184
 185         KKASSERT(a->priority < FAIRQ_MAXPRI);
 186         KKASSERT(a->qid != 0);
 187
 188         if (pif->pif_classes[a->priority] != NULL)
 189                 return (EBUSY);
 190         if (clh_to_clp(pif, a->qid) != NULL)
 191                 return (EBUSY);
 192
 193         cl = fairq_class_create(pif, a->priority, a->qlimit, a->bandwidth,
 194                                &a->pq_u.fairq_opts, a->qid);
 195         if (cl == NULL)
 196                 return (ENOMEM);
 197
 198         return (0);
 199 }
 200
 201 int
 202 fairq_add_queue(struct pf_altq *a)
 203 {
 204         struct fairq_if *pif;
 205         struct ifaltq *ifq;
 206         int error;
 207
 208         /* check parameters */
 209         if (a->priority >= FAIRQ_MAXPRI)
 210                 return (EINVAL);
 211         if (a->qid == 0)
 212                 return (EINVAL);
 213
 214         /* XXX not MP safe */
 215         if ((pif = a->altq_disc) == NULL)
 216                 return (EINVAL);
 217         ifq = pif->pif_ifq;
 218
 219         ALTQ_LOCK(ifq);
 220         error = fairq_add_queue_locked(a, pif);
 221         ALTQ_UNLOCK(ifq);
 222
 223         return error;
 224 }
 225
 226 static int
 227 fairq_remove_queue_locked(struct pf_altq *a, struct fairq_if *pif)
 228 {
 229         struct fairq_class *cl;
 230
 231         if ((cl = clh_to_clp(pif, a->qid)) == NULL)
 232                 return (EINVAL);
 233
 234         return (fairq_class_destroy(cl));
 235 }
 236
 237 int
 238 fairq_remove_queue(struct pf_altq *a)
 239 {
 240         struct fairq_if *pif;
 241         struct ifaltq *ifq;
 242         int error;
 243
 244         /* XXX not MP safe */
 245         if ((pif = a->altq_disc) == NULL)
 246                 return (EINVAL);
 247         ifq = pif->pif_ifq;
 248
 249         ALTQ_LOCK(ifq);
 250         error = fairq_remove_queue_locked(a, pif);
 251         ALTQ_UNLOCK(ifq);
 252
 253         return error;
 254 }
 255
 256 int
 257 fairq_getqstats(struct pf_altq *a, void *ubuf, int *nbytes)
 258 {
 259         struct fairq_if *pif;
 260         struct fairq_class *cl;
 261         struct fairq_classstats stats;
 262         struct ifaltq *ifq;
 263         int error = 0;
 264
 265         if (*nbytes < sizeof(stats))
 266                 return (EINVAL);
 267
 268         /* XXX not MP safe */
 269         if ((pif = altq_lookup(a->ifname, ALTQT_FAIRQ)) == NULL)
 270                 return (EBADF);
 271         ifq = pif->pif_ifq;
 272
 273         ALTQ_LOCK(ifq);
 274
 275         if ((cl = clh_to_clp(pif, a->qid)) == NULL) {
 276                 ALTQ_UNLOCK(ifq);
 277                 return (EINVAL);
 278         }
 279
 280         get_class_stats(&stats, cl);
 281
 282         ALTQ_UNLOCK(ifq);
 283
 284         if ((error = copyout((caddr_t)&stats, ubuf, sizeof(stats))) != 0)
 285                 return (error);
 286         *nbytes = sizeof(stats);
 287         return (0);
 288 }
 289
 290 /*
 291  * bring the interface back to the initial state by discarding
 292  * all the filters and classes.
 293  */
 294 static int
 295 fairq_clear_interface(struct fairq_if *pif)
 296 {
 297         struct fairq_class *cl;
 298         int pri;
 299
 300         /* clear out the classes */
 301         for (pri = 0; pri <= pif->pif_maxpri; pri++) {
 302                 if ((cl = pif->pif_classes[pri]) != NULL)
 303                         fairq_class_destroy(cl);
 304         }
 305
 306         return (0);
 307 }
 308
 309 static int
 310 fairq_request(struct ifaltq *ifq, int req, void *arg)
 311 {
 312         struct fairq_if *pif = (struct fairq_if *)ifq->altq_disc;
 313
 314         crit_enter();
 315         switch (req) {
 316         case ALTRQ_PURGE:
 317                 fairq_purge(pif);
 318                 break;
 319         }
 320         crit_exit();
 321         return (0);
 322 }
 323
 324 /* discard all the queued packets on the interface */
 325 static void
 326 fairq_purge(struct fairq_if *pif)
 327 {
 328         struct fairq_class *cl;
 329         int pri;
 330
 331         for (pri = 0; pri <= pif->pif_maxpri; pri++) {
 332                 if ((cl = pif->pif_classes[pri]) != NULL && cl->cl_head)
 333                         fairq_purgeq(cl);
 334         }
 335         if (ifq_is_enabled(pif->pif_ifq))
 336                 pif->pif_ifq->ifq_len = 0;
 337 }
 338
 339 static struct fairq_class *
 340 fairq_class_create(struct fairq_if *pif, int pri, int qlimit,
 341                    u_int bandwidth, struct fairq_opts *opts, int qid)
 342 {
 343         struct fairq_class *cl;
 344         int flags = opts->flags;
 345         u_int nbuckets = opts->nbuckets;
 346         int i;
 347
 348 #ifndef ALTQ_RED
 349         if (flags & FARF_RED) {
 350 #ifdef ALTQ_DEBUG
 351                 kprintf("fairq_class_create: RED not configured for FAIRQ!\n");
 352 #endif
 353                 return (NULL);
 354         }
 355 #endif
 356         if (nbuckets == 0)
 357                 nbuckets = 256;
 358         if (nbuckets > FAIRQ_MAX_BUCKETS)
 359                 nbuckets = FAIRQ_MAX_BUCKETS;
 360         /* enforce power-of-2 size */
 361         while ((nbuckets ^ (nbuckets - 1)) != ((nbuckets << 1) - 1))
 362                 ++nbuckets;
 363
 364         if ((cl = pif->pif_classes[pri]) != NULL) {
 365                 /* modify the class instead of creating a new one */
 366                 crit_enter();
 367                 if (cl->cl_head)
 368                         fairq_purgeq(cl);
 369                 crit_exit();
 370 #ifdef ALTQ_RIO
 371                 if (cl->cl_qtype == Q_RIO)
 372                         rio_destroy((rio_t *)cl->cl_red);
 373 #endif
 374 #ifdef ALTQ_RED
 375                 if (cl->cl_qtype == Q_RED)
 376                         red_destroy(cl->cl_red);
 377 #endif
 378         } else {
 379                 cl = kmalloc(sizeof(*cl), M_ALTQ, M_WAITOK | M_ZERO);
 380                 cl->cl_nbuckets = nbuckets;
 381                 cl->cl_nbucket_mask = nbuckets - 1;
 382
 383                 cl->cl_buckets = kmalloc(sizeof(*cl->cl_buckets) *
 384                                          cl->cl_nbuckets,
 385                                          M_ALTQ, M_WAITOK | M_ZERO);
 386                 cl->cl_head = NULL;
 387         }
 388
 389         pif->pif_classes[pri] = cl;
 390         if (flags & FARF_DEFAULTCLASS)
 391                 pif->pif_default = cl;
 392         if (qlimit == 0)
 393                 qlimit = 50;  /* use default */
 394         cl->cl_qlimit = qlimit;
 395         for (i = 0; i < cl->cl_nbuckets; ++i) {
 396                 qlimit(&cl->cl_buckets[i].queue) = qlimit;
 397         }
 398         cl->cl_bandwidth = bandwidth / 8;
 399         cl->cl_qtype = Q_DROPTAIL;
 400         cl->cl_flags = flags & FARF_USERFLAGS;
 401         cl->cl_pri = pri;
 402         if (pri > pif->pif_maxpri)
 403                 pif->pif_maxpri = pri;
 404         cl->cl_pif = pif;
 405         cl->cl_handle = qid;
 406         cl->cl_hogs_m1 = opts->hogs_m1 / 8;
 407         cl->cl_lssc_m1 = opts->lssc_m1 / 8;     /* NOT YET USED */
 408
 409 #ifdef ALTQ_RED
 410         if (flags & (FARF_RED|FARF_RIO)) {
 411                 int red_flags, red_pkttime;
 412
 413                 red_flags = 0;
 414                 if (flags & FARF_ECN)
 415                         red_flags |= REDF_ECN;
 416 #ifdef ALTQ_RIO
 417                 if (flags & FARF_CLEARDSCP)
 418                         red_flags |= RIOF_CLEARDSCP;
 419 #endif
 420                 if (pif->pif_bandwidth < 8)
 421                         red_pkttime = 1000 * 1000 * 1000; /* 1 sec */
 422                 else
 423                         red_pkttime = (int64_t)pif->pif_ifq->altq_ifp->if_mtu
 424                           * 1000 * 1000 * 1000 / (pif->pif_bandwidth / 8);
 425 #ifdef ALTQ_RIO
 426                 if (flags & FARF_RIO) {
 427                         cl->cl_red = (red_t *)rio_alloc(0, NULL,
 428                                                 red_flags, red_pkttime);
 429                         if (cl->cl_red != NULL)
 430                                 cl->cl_qtype = Q_RIO;
 431                 } else
 432 #endif
 433                 if (flags & FARF_RED) {
 434                         cl->cl_red = red_alloc(0, 0,
 435                             cl->cl_qlimit * 10/100,
 436                             cl->cl_qlimit * 30/100,
 437                             red_flags, red_pkttime);
 438                         if (cl->cl_red != NULL)
 439                                 cl->cl_qtype = Q_RED;
 440                 }
 441         }
 442 #endif /* ALTQ_RED */
 443
 444         return (cl);
 445 }
 446
 447 static int
 448 fairq_class_destroy(struct fairq_class *cl)
 449 {
 450         struct fairq_if *pif;
 451         int pri;
 452
 453         crit_enter();
 454
 455         if (cl->cl_head)
 456                 fairq_purgeq(cl);
 457
 458         pif = cl->cl_pif;
 459         pif->pif_classes[cl->cl_pri] = NULL;
 460         if (pif->pif_poll_cache == cl)
 461                 pif->pif_poll_cache = NULL;
 462         if (pif->pif_maxpri == cl->cl_pri) {
 463                 for (pri = cl->cl_pri; pri >= 0; pri--)
 464                         if (pif->pif_classes[pri] != NULL) {
 465                                 pif->pif_maxpri = pri;
 466                                 break;
 467                         }
 468                 if (pri < 0)
 469                         pif->pif_maxpri = -1;
 470         }
 471         crit_exit();
 472
 473         if (cl->cl_red != NULL) {
 474 #ifdef ALTQ_RIO
 475                 if (cl->cl_qtype == Q_RIO)
 476                         rio_destroy((rio_t *)cl->cl_red);
 477 #endif
 478 #ifdef ALTQ_RED
 479                 if (cl->cl_qtype == Q_RED)
 480                         red_destroy(cl->cl_red);
 481 #endif
 482         }
 483         kfree(cl->cl_buckets, M_ALTQ);
 484         cl->cl_head = NULL;     /* sanity */
 485         cl->cl_polled = NULL;   /* sanity */
 486         cl->cl_buckets = NULL;  /* sanity */
 487         kfree(cl, M_ALTQ);
 488
 489         return (0);
 490 }
 491
 492 /*
 493  * fairq_enqueue is an enqueue function to be registered to
 494  * (*altq_enqueue) in struct ifaltq.
 495  */
 496 static int
 497 fairq_enqueue(struct ifaltq *ifq, struct mbuf *m, struct altq_pktattr *pktattr)
 498 {
 499         struct fairq_if *pif = (struct fairq_if *)ifq->altq_disc;
 500         struct fairq_class *cl;
 501         int error;
 502         int len;
 503
 504         crit_enter();
 505
 506         /* grab class set by classifier */
 507         if ((m->m_flags & M_PKTHDR) == 0) {
 508                 /* should not happen */
 509                 if_printf(ifq->altq_ifp, "altq: packet does not have pkthdr\n");
 510                 m_freem(m);
 511                 error = ENOBUFS;
 512                 goto done;
 513         }
 514
 515         if (m->m_pkthdr.fw_flags & ALTQ_MBUF_TAGGED)
 516                 cl = clh_to_clp(pif, m->m_pkthdr.altq_qid);
 517         else
 518                 cl = NULL;
 519         if (cl == NULL) {
 520                 cl = pif->pif_default;
 521                 if (cl == NULL) {
 522                         m_freem(m);
 523                         error = ENOBUFS;
 524                         goto done;
 525                 }
 526         }
 527         cl->cl_flags |= FARF_HAS_PACKETS;
 528         cl->cl_pktattr = NULL;
 529         len = m_pktlen(m);
 530         if (fairq_addq(cl, m) != 0) {
 531                 /* drop occurred.  mbuf was freed in fairq_addq. */
 532                 PKTCNTR_ADD(&cl->cl_dropcnt, len);
 533                 error = ENOBUFS;
 534                 goto done;
 535         }
 536         ifq->ifq_len++;
 537         error = 0;
 538 done:
 539         crit_exit();
 540         return (error);
 541 }
 542
 543 /*
 544  * fairq_dequeue is a dequeue function to be registered to
 545  * (*altq_dequeue) in struct ifaltq.
 546  *
 547  * note: ALTDQ_POLL returns the next packet without removing the packet
 548  *      from the queue.  ALTDQ_REMOVE is a normal dequeue operation.
 549  *      ALTDQ_REMOVE must return the same packet if called immediately
 550  *      after ALTDQ_POLL.
 551  */
 552 static struct mbuf *
 553 fairq_dequeue(struct ifaltq *ifq, struct mbuf *mpolled, int op)
 554 {
 555         struct fairq_if *pif = (struct fairq_if *)ifq->altq_disc;
 556         struct fairq_class *cl;
 557         struct fairq_class *best_cl;
 558         struct mbuf *best_m;
 559         struct mbuf *m;
 560         uint64_t cur_time = read_machclk();
 561         int pri;
 562         int hit_limit;
 563
 564         if (ifq_is_empty(ifq)) {
 565                 /* no packet in the queue */
 566                 KKASSERT(mpolled == NULL);
 567                 return (NULL);
 568         }
 569
 570         crit_enter();
 571         if (pif->pif_poll_cache && op == ALTDQ_REMOVE) {
 572                 best_cl = pif->pif_poll_cache;
 573                 m = fairq_getq(best_cl, cur_time);
 574                 pif->pif_poll_cache = NULL;
 575                 if (m) {
 576                         ifq->ifq_len--;
 577                         PKTCNTR_ADD(&best_cl->cl_xmitcnt, m_pktlen(m));
 578                 }
 579         } else {
 580                 best_cl = NULL;
 581                 best_m = NULL;
 582
 583                 for (pri = pif->pif_maxpri;  pri >= 0; pri--) {
 584                         if ((cl = pif->pif_classes[pri]) == NULL)
 585                                 continue;
 586                         if ((cl->cl_flags & FARF_HAS_PACKETS) == 0)
 587                                 continue;
 588                         m = fairq_pollq(cl, cur_time, &hit_limit);
 589                         if (m == NULL) {
 590                                 cl->cl_flags &= ~FARF_HAS_PACKETS;
 591                                 continue;
 592                         }
 593
 594                         /*
 595                          * Only override the best choice if we are under
 596                          * the BW limit.
 597                          */
 598                         if (hit_limit == 0 || best_cl == NULL) {
 599                                 best_cl = cl;
 600                                 best_m = m;
 601                         }
 602
 603                         /*
 604                          * Remember the highest priority mbuf in case we
 605                          * do not find any lower priority mbufs.
 606                          */
 607                         if (hit_limit)
 608                                 continue;
 609                         break;
 610                 }
 611                 if (op == ALTDQ_POLL) {
 612                         pif->pif_poll_cache = best_cl;
 613                         m = best_m;
 614                 } else if (best_cl) {
 615                         m = fairq_getq(best_cl, cur_time);
 616                         KKASSERT(best_m == m);
 617                         ifq->ifq_len--;
 618                         PKTCNTR_ADD(&best_cl->cl_xmitcnt, m_pktlen(m));
 619                 } else {
 620                         m = NULL;
 621                 }
 622         }
 623         crit_exit();
 624         KKASSERT(mpolled == NULL || mpolled == m);
 625         return (m);
 626 }
 627
 628 static int
 629 fairq_addq(struct fairq_class *cl, struct mbuf *m)
 630 {
 631         fairq_bucket_t *b;
 632         u_int hindex;
 633         uint64_t bw;
 634
 635         /*
 636          * If the packet doesn't have any keep state put it on the end of
 637          * our queue.  XXX this can result in out of order delivery.
 638          */
 639         if ((m->m_pkthdr.fw_flags & ALTQ_MBUF_STATE_HASHED) == 0) {
 640                 if (cl->cl_head)
 641                         b = cl->cl_head->prev;
 642                 else
 643                         b = &cl->cl_buckets[0];
 644         } else {
 645                 hindex = m->m_pkthdr.altq_state_hash & cl->cl_nbucket_mask;
 646                 b = &cl->cl_buckets[hindex];
 647         }
 648
 649         /*
 650          * Add the bucket to the end of the circular list of active buckets.
 651          *
 652          * As a special case we add the bucket to the beginning of the list
 653          * instead of the end if it was not previously on the list and if
 654          * its traffic is less then the hog level.
 655          */
 656         if (b->in_use == 0) {
 657                 b->in_use = 1;
 658                 if (cl->cl_head == NULL) {
 659                         cl->cl_head = b;
 660                         b->next = b;
 661                         b->prev = b;
 662                 } else {
 663                         b->next = cl->cl_head;
 664                         b->prev = cl->cl_head->prev;
 665                         b->prev->next = b;
 666                         b->next->prev = b;
 667
 668                         if (b->bw_delta && cl->cl_hogs_m1) {
 669                                 bw = b->bw_bytes * machclk_freq / b->bw_delta;
 670                                 if (bw < cl->cl_hogs_m1)
 671                                         cl->cl_head = b;
 672                         }
 673                 }
 674         }
 675
 676 #ifdef ALTQ_RIO
 677         if (cl->cl_qtype == Q_RIO)
 678                 return rio_addq((rio_t *)cl->cl_red, &b->queue, m, cl->cl_pktattr);
 679 #endif
 680 #ifdef ALTQ_RED
 681         if (cl->cl_qtype == Q_RED)
 682                 return red_addq(cl->cl_red, &b->queue, m, cl->cl_pktattr);
 683 #endif
 684         if (qlen(&b->queue) >= qlimit(&b->queue)) {
 685                 m_freem(m);
 686                 return (-1);
 687         }
 688
 689         if (cl->cl_flags & FARF_CLEARDSCP)
 690                 write_dsfield(m, cl->cl_pktattr, 0);
 691
 692         _addq(&b->queue, m);
 693
 694         return (0);
 695 }
 696
 697 static struct mbuf *
 698 fairq_getq(struct fairq_class *cl, uint64_t cur_time)
 699 {
 700         fairq_bucket_t *b;
 701         struct mbuf *m;
 702
 703         b = fairq_selectq(cl, 0);
 704         if (b == NULL)
 705                 m = NULL;
 706 #ifdef ALTQ_RIO
 707         else if (cl->cl_qtype == Q_RIO)
 708                 m = rio_getq((rio_t *)cl->cl_red, &b->queue);
 709 #endif
 710 #ifdef ALTQ_RED
 711         else if (cl->cl_qtype == Q_RED)
 712                 m = red_getq(cl->cl_red, &b->queue);
 713 #endif
 714         else
 715                 m = _getq(&b->queue);
 716
 717         /*
 718          * Calculate the BW change
 719          */
 720         if (m != NULL) {
 721                 uint64_t delta;
 722
 723                 /*
 724                  * Per-class bandwidth calculation
 725                  */
 726                 delta = (cur_time - cl->cl_last_time);
 727                 if (delta > machclk_freq * 8)
 728                         delta = machclk_freq * 8;
 729                 cl->cl_bw_delta += delta;
 730                 cl->cl_bw_bytes += m->m_pkthdr.len;
 731                 cl->cl_last_time = cur_time;
 732                 cl->cl_bw_delta -= cl->cl_bw_delta >> 3;
 733                 cl->cl_bw_bytes -= cl->cl_bw_bytes >> 3;
 734
 735                 /*
 736                  * Per-bucket bandwidth calculation
 737                  */
 738                 delta = (cur_time - b->last_time);
 739                 if (delta > machclk_freq * 8)
 740                         delta = machclk_freq * 8;
 741                 b->bw_delta += delta;
 742                 b->bw_bytes += m->m_pkthdr.len;
 743                 b->last_time = cur_time;
 744                 b->bw_delta -= b->bw_delta >> 3;
 745                 b->bw_bytes -= b->bw_bytes >> 3;
 746         }
 747         return(m);
 748 }
 749
 750 /*
 751  * Figure out what the next packet would be if there were no limits.  If
 752  * this class hits its bandwidth limit *hit_limit is set to no-zero, otherwise
 753  * it is set to 0.  A non-NULL mbuf is returned either way.
 754  */
 755 static struct mbuf *
 756 fairq_pollq(struct fairq_class *cl, uint64_t cur_time, int *hit_limit)
 757 {
 758         fairq_bucket_t *b;
 759         struct mbuf *m;
 760         uint64_t delta;
 761         uint64_t bw;
 762
 763         *hit_limit = 0;
 764         b = fairq_selectq(cl, 1);
 765         if (b == NULL)
 766                 return(NULL);
 767         m = qhead(&b->queue);
 768
 769         /*
 770          * Did this packet exceed the class bandwidth?  Calculate the
 771          * bandwidth component of the packet.
 772          *
 773          * - Calculate bytes per second
 774          */
 775         delta = cur_time - cl->cl_last_time;
 776         if (delta > machclk_freq * 8)
 777                 delta = machclk_freq * 8;
 778         cl->cl_bw_delta += delta;
 779         cl->cl_last_time = cur_time;
 780         if (cl->cl_bw_delta) {
 781                 bw = cl->cl_bw_bytes * machclk_freq / cl->cl_bw_delta;
 782
 783                 if (bw > cl->cl_bandwidth)
 784                         *hit_limit = 1;
 785 #if 0
 786                 kprintf("BW %6lld relative to %6u %d queue %p\n",
 787                         bw, cl->cl_bandwidth, *hit_limit, b);
 788 #endif
 789         }
 790         return(m);
 791 }
 792
 793 /*
 794  * Locate the next queue we want to pull a packet out of.  This code
 795  * is also responsible for removing empty buckets from the circular list.
 796  */
 797 static
 798 fairq_bucket_t *
 799 fairq_selectq(struct fairq_class *cl, int ispoll)
 800 {
 801         fairq_bucket_t *b;
 802         uint64_t bw;
 803
 804         if (ispoll == 0 && cl->cl_polled) {
 805                 b = cl->cl_polled;
 806                 cl->cl_polled = NULL;
 807                 return(b);
 808         }
 809
 810         while ((b = cl->cl_head) != NULL) {
 811                 /*
 812                  * Remove empty queues from consideration
 813                  */
 814                 if (qempty(&b->queue)) {
 815                         b->in_use = 0;
 816                         cl->cl_head = b->next;
 817                         if (cl->cl_head == b) {
 818                                 cl->cl_head = NULL;
 819                         } else {
 820                                 b->next->prev = b->prev;
 821                                 b->prev->next = b->next;
 822                         }
 823                         continue;
 824                 }
 825
 826                 /*
 827                  * Advance the round robin.  Queues with bandwidths less
 828                  * then the hog bandwidth are allowed to burst.
 829                  */
 830                 if (cl->cl_hogs_m1 == 0) {
 831                         cl->cl_head = b->next;
 832                 } else if (b->bw_delta) {
 833                         bw = b->bw_bytes * machclk_freq / b->bw_delta;
 834                         if (bw >= cl->cl_hogs_m1) {
 835                                 cl->cl_head = b->next;
 836                         }
 837                         /*
 838                          * XXX TODO -
 839                          */
 840                 }
 841
 842                 /*
 843                  * Return bucket b.
 844                  */
 845                 break;
 846         }
 847         if (ispoll)
 848                 cl->cl_polled = b;
 849         return(b);
 850 }
 851
 852 static void
 853 fairq_purgeq(struct fairq_class *cl)
 854 {
 855         fairq_bucket_t *b;
 856         struct mbuf *m;
 857
 858         while ((b = fairq_selectq(cl, 0)) != NULL) {
 859                 while ((m = _getq(&b->queue)) != NULL) {
 860                         PKTCNTR_ADD(&cl->cl_dropcnt, m_pktlen(m));
 861                         m_freem(m);
 862                 }
 863                 KKASSERT(qlen(&b->queue) == 0);
 864         }
 865 }
 866
 867 static void
 868 get_class_stats(struct fairq_classstats *sp, struct fairq_class *cl)
 869 {
 870         fairq_bucket_t *b;
 871
 872         sp->class_handle = cl->cl_handle;
 873         sp->qlimit = cl->cl_qlimit;
 874         sp->xmit_cnt = cl->cl_xmitcnt;
 875         sp->drop_cnt = cl->cl_dropcnt;
 876         sp->qtype = cl->cl_qtype;
 877         sp->qlength = 0;
 878
 879         if (cl->cl_head) {
 880                 b = cl->cl_head;
 881                 do {
 882                         sp->qlength += qlen(&b->queue);
 883                         b = b->next;
 884                 } while (b != cl->cl_head);
 885         }
 886
 887 #ifdef ALTQ_RED
 888         if (cl->cl_qtype == Q_RED)
 889                 red_getstats(cl->cl_red, &sp->red[0]);
 890 #endif
 891 #ifdef ALTQ_RIO
 892         if (cl->cl_qtype == Q_RIO)
 893                 rio_getstats((rio_t *)cl->cl_red, &sp->red[0]);
 894 #endif
 895 }
 896
 897 /* convert a class handle to the corresponding class pointer */
 898 static struct fairq_class *
 899 clh_to_clp(struct fairq_if *pif, uint32_t chandle)
 900 {
 901         struct fairq_class *cl;
 902         int idx;
 903
 904         if (chandle == 0)
 905                 return (NULL);
 906
 907         for (idx = pif->pif_maxpri; idx >= 0; idx--)
 908                 if ((cl = pif->pif_classes[idx]) != NULL &&
 909                     cl->cl_handle == chandle)
 910                         return (cl);
 911
 912         return (NULL);
 913 }
 914
 915 #endif /* ALTQ_FAIRQ */