sys/contrib/altq/altq/altq_hfsc.c

   1 /*      $FreeBSD$       */
   2 /*      $KAME: altq_hfsc.c,v 1.24 2003/12/05 05:40:46 kjc Exp $ */
   3
   4 /*
   5  * Copyright (c) 1997-1999 Carnegie Mellon University. All Rights Reserved.
   6  *
   7  * Permission to use, copy, modify, and distribute this software and
   8  * its documentation is hereby granted (including for commercial or
   9  * for-profit use), provided that both the copyright notice and this
  10  * permission notice appear in all copies of the software, derivative
  11  * works, or modified versions, and any portions thereof.
  12  *
  13  * THIS SOFTWARE IS EXPERIMENTAL AND IS KNOWN TO HAVE BUGS, SOME OF
  14  * WHICH MAY HAVE SERIOUS CONSEQUENCES.  CARNEGIE MELLON PROVIDES THIS
  15  * SOFTWARE IN ITS ``AS IS'' CONDITION, AND ANY EXPRESS OR IMPLIED
  16  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  17  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  18  * DISCLAIMED.  IN NO EVENT SHALL CARNEGIE MELLON UNIVERSITY BE LIABLE
  19  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  20  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
  21  * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
  22  * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  23  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
  25  * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  26  * DAMAGE.
  27  *
  28  * Carnegie Mellon encourages (but does not require) users of this
  29  * software to return any improvements or extensions that they make,
  30  * and to grant Carnegie Mellon the rights to redistribute these
  31  * changes without encumbrance.
  32  */
  33 /*
  34  * H-FSC is described in Proceedings of SIGCOMM'97,
  35  * "A Hierarchical Fair Service Curve Algorithm for Link-Sharing,
  36  * Real-Time and Priority Service"
  37  * by Ion Stoica, Hui Zhang, and T. S. Eugene Ng.
  38  *
  39  * Oleg Cherevko <olwi@aq.ml.com.ua> added the upperlimit for link-sharing.
  40  * when a class has an upperlimit, the fit-time is computed from the
  41  * upperlimit service curve.  the link-sharing scheduler does not schedule
  42  * a class whose fit-time exceeds the current time.
  43  */
  44
  45 #if defined(__FreeBSD__) || defined(__NetBSD__)
  46 #include "opt_altq.h"
  47 #if (__FreeBSD__ != 2)
  48 #include "opt_inet.h"
  49 #ifdef __FreeBSD__
  50 #include "opt_inet6.h"
  51 #endif
  52 #endif
  53 #endif /* __FreeBSD__ || __NetBSD__ */
  54
  55 #ifdef ALTQ_HFSC  /* hfsc is enabled by ALTQ_HFSC option in opt_altq.h */
  56
  57 #include <sys/param.h>
  58 #include <sys/malloc.h>
  59 #include <sys/mbuf.h>
  60 #include <sys/socket.h>
  61 #include <sys/systm.h>
  62 #include <sys/errno.h>
  63 #include <sys/queue.h>
  64 #if 1 /* ALTQ3_COMPAT */
  65 #include <sys/sockio.h>
  66 #include <sys/proc.h>
  67 #include <sys/kernel.h>
  68 #endif /* ALTQ3_COMPAT */
  69
  70 #include <net/if.h>
  71 #include <netinet/in.h>
  72
  73 #include <net/pfvar.h>
  74 #include <altq/altq.h>
  75 #include <altq/altq_hfsc.h>
  76 #ifdef ALTQ3_COMPAT
  77 #include <altq/altq_conf.h>
  78 #endif
  79
  80 /*
  81  * function prototypes
  82  */
  83 static int                       hfsc_clear_interface(struct hfsc_if *);
  84 static int                       hfsc_request(struct ifaltq *, int, void *);
  85 static void                      hfsc_purge(struct hfsc_if *);
  86 static struct hfsc_class        *hfsc_class_create(struct hfsc_if *,
  87     struct service_curve *, struct service_curve *, struct service_curve *,
  88     struct hfsc_class *, int, int, int);
  89 static int                       hfsc_class_destroy(struct hfsc_class *);
  90 static struct hfsc_class        *hfsc_nextclass(struct hfsc_class *);
  91 static int                       hfsc_enqueue(struct ifaltq *, struct mbuf *,
  92                                     struct altq_pktattr *);
  93 static struct mbuf              *hfsc_dequeue(struct ifaltq *, int);
  94
  95 static int               hfsc_addq(struct hfsc_class *, struct mbuf *);
  96 static struct mbuf      *hfsc_getq(struct hfsc_class *);
  97 static struct mbuf      *hfsc_pollq(struct hfsc_class *);
  98 static void              hfsc_purgeq(struct hfsc_class *);
  99
 100 static void              update_cfmin(struct hfsc_class *);
 101 static void              set_active(struct hfsc_class *, int);
 102 static void              set_passive(struct hfsc_class *);
 103
 104 static void              init_ed(struct hfsc_class *, int);
 105 static void              update_ed(struct hfsc_class *, int);
 106 static void              update_d(struct hfsc_class *, int);
 107 static void              init_vf(struct hfsc_class *, int);
 108 static void              update_vf(struct hfsc_class *, int, u_int64_t);
 109 static ellist_t         *ellist_alloc(void);
 110 static void              ellist_destroy(ellist_t *);
 111 static void              ellist_insert(struct hfsc_class *);
 112 static void              ellist_remove(struct hfsc_class *);
 113 static void              ellist_update(struct hfsc_class *);
 114 struct hfsc_class       *ellist_get_mindl(ellist_t *, u_int64_t);
 115 static actlist_t        *actlist_alloc(void);
 116 static void              actlist_destroy(actlist_t *);
 117 static void              actlist_insert(struct hfsc_class *);
 118 static void              actlist_remove(struct hfsc_class *);
 119 static void              actlist_update(struct hfsc_class *);
 120
 121 static struct hfsc_class        *actlist_firstfit(struct hfsc_class *,
 122                                     u_int64_t);
 123
 124 static __inline u_int64_t       seg_x2y(u_int64_t, u_int64_t);
 125 static __inline u_int64_t       seg_y2x(u_int64_t, u_int64_t);
 126 static __inline u_int64_t       m2sm(u_int);
 127 static __inline u_int64_t       m2ism(u_int);
 128 static __inline u_int64_t       d2dx(u_int);
 129 static u_int                    sm2m(u_int64_t);
 130 static u_int                    dx2d(u_int64_t);
 131
 132 static void             sc2isc(struct service_curve *, struct internal_sc *);
 133 static void             rtsc_init(struct runtime_sc *, struct internal_sc *,
 134                             u_int64_t, u_int64_t);
 135 static u_int64_t        rtsc_y2x(struct runtime_sc *, u_int64_t);
 136 static u_int64_t        rtsc_x2y(struct runtime_sc *, u_int64_t);
 137 static void             rtsc_min(struct runtime_sc *, struct internal_sc *,
 138                             u_int64_t, u_int64_t);
 139
 140 static void                      get_class_stats(struct hfsc_classstats *,
 141                                     struct hfsc_class *);
 142 static struct hfsc_class        *clh_to_clp(struct hfsc_if *, u_int32_t);
 143
 144
 145 #ifdef ALTQ3_COMPAT
 146 static struct hfsc_if *hfsc_attach(struct ifaltq *, u_int);
 147 static int hfsc_detach(struct hfsc_if *);
 148 static int hfsc_class_modify(struct hfsc_class *, struct service_curve *,
 149     struct service_curve *, struct service_curve *);
 150
 151 static int hfsccmd_if_attach(struct hfsc_attach *);
 152 static int hfsccmd_if_detach(struct hfsc_interface *);
 153 static int hfsccmd_add_class(struct hfsc_add_class *);
 154 static int hfsccmd_delete_class(struct hfsc_delete_class *);
 155 static int hfsccmd_modify_class(struct hfsc_modify_class *);
 156 static int hfsccmd_add_filter(struct hfsc_add_filter *);
 157 static int hfsccmd_delete_filter(struct hfsc_delete_filter *);
 158 static int hfsccmd_class_stats(struct hfsc_class_stats *);
 159
 160 altqdev_decl(hfsc);
 161 #endif /* ALTQ3_COMPAT */
 162
 163 /*
 164  * macros
 165  */
 166 #define is_a_parent_class(cl)   ((cl)->cl_children != NULL)
 167
 168 #define HT_INFINITY     0xffffffffffffffffLL    /* infinite time value */
 169
 170 #ifdef ALTQ3_COMPAT
 171 /* hif_list keeps all hfsc_if's allocated. */
 172 static struct hfsc_if *hif_list = NULL;
 173 #endif /* ALTQ3_COMPAT */
 174
 175 int
 176 hfsc_pfattach(struct pf_altq *a)
 177 {
 178         struct ifnet *ifp;
 179         int s, error;
 180
 181         if ((ifp = ifunit(a->ifname)) == NULL || a->altq_disc == NULL)
 182                 return (EINVAL);
 183 #ifdef __NetBSD__
 184         s = splnet();
 185 #else
 186         s = splimp();
 187 #endif
 188         error = altq_attach(&ifp->if_snd, ALTQT_HFSC, a->altq_disc,
 189             hfsc_enqueue, hfsc_dequeue, hfsc_request, NULL, NULL);
 190         splx(s);
 191         return (error);
 192 }
 193
 194 int
 195 hfsc_add_altq(struct pf_altq *a)
 196 {
 197         struct hfsc_if *hif;
 198         struct ifnet *ifp;
 199
 200         if ((ifp = ifunit(a->ifname)) == NULL)
 201                 return (EINVAL);
 202         if (!ALTQ_IS_READY(&ifp->if_snd))
 203                 return (ENODEV);
 204
 205         hif = malloc(sizeof(struct hfsc_if), M_DEVBUF, M_WAITOK);
 206         if (hif == NULL)
 207                 return (ENOMEM);
 208         bzero(hif, sizeof(struct hfsc_if));
 209
 210         hif->hif_eligible = ellist_alloc();
 211         if (hif->hif_eligible == NULL) {
 212                 free(hif, M_DEVBUF);
 213                 return (ENOMEM);
 214         }
 215
 216         hif->hif_ifq = &ifp->if_snd;
 217
 218         /* keep the state in pf_altq */
 219         a->altq_disc = hif;
 220
 221         return (0);
 222 }
 223
 224 int
 225 hfsc_remove_altq(struct pf_altq *a)
 226 {
 227         struct hfsc_if *hif;
 228
 229         if ((hif = a->altq_disc) == NULL)
 230                 return (EINVAL);
 231         a->altq_disc = NULL;
 232
 233         (void)hfsc_clear_interface(hif);
 234         (void)hfsc_class_destroy(hif->hif_rootclass);
 235
 236         ellist_destroy(hif->hif_eligible);
 237
 238         free(hif, M_DEVBUF);
 239
 240         return (0);
 241 }
 242
 243 int
 244 hfsc_add_queue(struct pf_altq *a)
 245 {
 246         struct hfsc_if *hif;
 247         struct hfsc_class *cl, *parent;
 248         struct hfsc_opts *opts;
 249         struct service_curve rtsc, lssc, ulsc;
 250
 251         if ((hif = a->altq_disc) == NULL)
 252                 return (EINVAL);
 253
 254         opts = &a->pq_u.hfsc_opts;
 255
 256         if (a->parent_qid == HFSC_NULLCLASS_HANDLE &&
 257             hif->hif_rootclass == NULL)
 258                 parent = NULL;
 259         else if ((parent = clh_to_clp(hif, a->parent_qid)) == NULL)
 260                 return (EINVAL);
 261
 262         if (a->qid == 0)
 263                 return (EINVAL);
 264
 265         if (clh_to_clp(hif, a->qid) != NULL)
 266                 return (EBUSY);
 267
 268         rtsc.m1 = opts->rtsc_m1;
 269         rtsc.d  = opts->rtsc_d;
 270         rtsc.m2 = opts->rtsc_m2;
 271         lssc.m1 = opts->lssc_m1;
 272         lssc.d  = opts->lssc_d;
 273         lssc.m2 = opts->lssc_m2;
 274         ulsc.m1 = opts->ulsc_m1;
 275         ulsc.d  = opts->ulsc_d;
 276         ulsc.m2 = opts->ulsc_m2;
 277
 278         cl = hfsc_class_create(hif, &rtsc, &lssc, &ulsc,
 279             parent, a->qlimit, opts->flags, a->qid);
 280         if (cl == NULL)
 281                 return (ENOMEM);
 282
 283         return (0);
 284 }
 285
 286 int
 287 hfsc_remove_queue(struct pf_altq *a)
 288 {
 289         struct hfsc_if *hif;
 290         struct hfsc_class *cl;
 291
 292         if ((hif = a->altq_disc) == NULL)
 293                 return (EINVAL);
 294
 295         if ((cl = clh_to_clp(hif, a->qid)) == NULL)
 296                 return (EINVAL);
 297
 298         return (hfsc_class_destroy(cl));
 299 }
 300
 301 int
 302 hfsc_getqstats(struct pf_altq *a, void *ubuf, int *nbytes)
 303 {
 304         struct hfsc_if *hif;
 305         struct hfsc_class *cl;
 306         struct hfsc_classstats stats;
 307         int error = 0;
 308
 309         if ((hif = altq_lookup(a->ifname, ALTQT_HFSC)) == NULL)
 310                 return (EBADF);
 311
 312         if ((cl = clh_to_clp(hif, a->qid)) == NULL)
 313                 return (EINVAL);
 314
 315         if (*nbytes < sizeof(stats))
 316                 return (EINVAL);
 317
 318         get_class_stats(&stats, cl);
 319
 320         if ((error = copyout((caddr_t)&stats, ubuf, sizeof(stats))) != 0)
 321                 return (error);
 322         *nbytes = sizeof(stats);
 323         return (0);
 324 }
 325
 326 /*
 327  * bring the interface back to the initial state by discarding
 328  * all the filters and classes except the root class.
 329  */
 330 static int
 331 hfsc_clear_interface(struct hfsc_if *hif)
 332 {
 333         struct hfsc_class       *cl;
 334
 335 #ifdef ALTQ3_COMPAT
 336         /* free the filters for this interface */
 337         acc_discard_filters(&hif->hif_classifier, NULL, 1);
 338 #endif
 339
 340         /* clear out the classes */
 341         while (hif->hif_rootclass != NULL &&
 342             (cl = hif->hif_rootclass->cl_children) != NULL) {
 343                 /*
 344                  * remove the first leaf class found in the hierarchy
 345                  * then start over
 346                  */
 347                 for (; cl != NULL; cl = hfsc_nextclass(cl)) {
 348                         if (!is_a_parent_class(cl)) {
 349                                 (void)hfsc_class_destroy(cl);
 350                                 break;
 351                         }
 352                 }
 353         }
 354
 355         return (0);
 356 }
 357
 358 static int
 359 hfsc_request(struct ifaltq *ifq, int req, void *arg)
 360 {
 361         struct hfsc_if  *hif = (struct hfsc_if *)ifq->altq_disc;
 362
 363         IFQ_LOCK_ASSERT(ifq);
 364
 365         switch (req) {
 366         case ALTRQ_PURGE:
 367                 hfsc_purge(hif);
 368                 break;
 369         }
 370         return (0);
 371 }
 372
 373 /* discard all the queued packets on the interface */
 374 static void
 375 hfsc_purge(struct hfsc_if *hif)
 376 {
 377         struct hfsc_class *cl;
 378
 379         for (cl = hif->hif_rootclass; cl != NULL; cl = hfsc_nextclass(cl))
 380                 if (!qempty(cl->cl_q))
 381                         hfsc_purgeq(cl);
 382         if (ALTQ_IS_ENABLED(hif->hif_ifq))
 383                 hif->hif_ifq->ifq_len = 0;
 384 }
 385
 386 struct hfsc_class *
 387 hfsc_class_create(struct hfsc_if *hif, struct service_curve *rsc,
 388     struct service_curve *fsc, struct service_curve *usc,
 389     struct hfsc_class *parent, int qlimit, int flags, int qid)
 390 {
 391         struct hfsc_class *cl, *p;
 392         int i, s;
 393
 394         if (hif->hif_classes >= HFSC_MAX_CLASSES)
 395                 return (NULL);
 396
 397 #ifndef ALTQ_RED
 398         if (flags & HFCF_RED) {
 399 #ifdef ALTQ_DEBUG
 400                 printf("hfsc_class_create: RED not configured for HFSC!\n");
 401 #endif
 402                 return (NULL);
 403         }
 404 #endif
 405
 406         cl = malloc(sizeof(struct hfsc_class), M_DEVBUF, M_WAITOK);
 407         if (cl == NULL)
 408                 return (NULL);
 409         bzero(cl, sizeof(struct hfsc_class));
 410
 411         cl->cl_q = malloc(sizeof(class_queue_t), M_DEVBUF, M_WAITOK);
 412         if (cl->cl_q == NULL)
 413                 goto err_ret;
 414         bzero(cl->cl_q, sizeof(class_queue_t));
 415
 416         cl->cl_actc = actlist_alloc();
 417         if (cl->cl_actc == NULL)
 418                 goto err_ret;
 419
 420         if (qlimit == 0)
 421                 qlimit = 50;  /* use default */
 422         qlimit(cl->cl_q) = qlimit;
 423         qtype(cl->cl_q) = Q_DROPTAIL;
 424         qlen(cl->cl_q) = 0;
 425         cl->cl_flags = flags;
 426 #ifdef ALTQ_RED
 427         if (flags & (HFCF_RED|HFCF_RIO)) {
 428                 int red_flags, red_pkttime;
 429                 u_int m2;
 430
 431                 m2 = 0;
 432                 if (rsc != NULL && rsc->m2 > m2)
 433                         m2 = rsc->m2;
 434                 if (fsc != NULL && fsc->m2 > m2)
 435                         m2 = fsc->m2;
 436                 if (usc != NULL && usc->m2 > m2)
 437                         m2 = usc->m2;
 438
 439                 red_flags = 0;
 440                 if (flags & HFCF_ECN)
 441                         red_flags |= REDF_ECN;
 442 #ifdef ALTQ_RIO
 443                 if (flags & HFCF_CLEARDSCP)
 444                         red_flags |= RIOF_CLEARDSCP;
 445 #endif
 446                 if (m2 < 8)
 447                         red_pkttime = 1000 * 1000 * 1000; /* 1 sec */
 448                 else
 449                         red_pkttime = (int64_t)hif->hif_ifq->altq_ifp->if_mtu
 450                                 * 1000 * 1000 * 1000 / (m2 / 8);
 451                 if (flags & HFCF_RED) {
 452                         cl->cl_red = red_alloc(0, 0,
 453                             qlimit(cl->cl_q) * 10/100,
 454                             qlimit(cl->cl_q) * 30/100,
 455                             red_flags, red_pkttime);
 456                         if (cl->cl_red != NULL)
 457                                 qtype(cl->cl_q) = Q_RED;
 458                 }
 459 #ifdef ALTQ_RIO
 460                 else {
 461                         cl->cl_red = (red_t *)rio_alloc(0, NULL,
 462                             red_flags, red_pkttime);
 463                         if (cl->cl_red != NULL)
 464                                 qtype(cl->cl_q) = Q_RIO;
 465                 }
 466 #endif
 467         }
 468 #endif /* ALTQ_RED */
 469
 470         if (rsc != NULL && (rsc->m1 != 0 || rsc->m2 != 0)) {
 471                 cl->cl_rsc = malloc(sizeof(struct internal_sc),
 472                     M_DEVBUF, M_WAITOK);
 473                 if (cl->cl_rsc == NULL)
 474                         goto err_ret;
 475                 sc2isc(rsc, cl->cl_rsc);
 476                 rtsc_init(&cl->cl_deadline, cl->cl_rsc, 0, 0);
 477                 rtsc_init(&cl->cl_eligible, cl->cl_rsc, 0, 0);
 478         }
 479         if (fsc != NULL && (fsc->m1 != 0 || fsc->m2 != 0)) {
 480                 cl->cl_fsc = malloc(sizeof(struct internal_sc),
 481                     M_DEVBUF, M_WAITOK);
 482                 if (cl->cl_fsc == NULL)
 483                         goto err_ret;
 484                 sc2isc(fsc, cl->cl_fsc);
 485                 rtsc_init(&cl->cl_virtual, cl->cl_fsc, 0, 0);
 486         }
 487         if (usc != NULL && (usc->m1 != 0 || usc->m2 != 0)) {
 488                 cl->cl_usc = malloc(sizeof(struct internal_sc),
 489                     M_DEVBUF, M_WAITOK);
 490                 if (cl->cl_usc == NULL)
 491                         goto err_ret;
 492                 sc2isc(usc, cl->cl_usc);
 493                 rtsc_init(&cl->cl_ulimit, cl->cl_usc, 0, 0);
 494         }
 495
 496         cl->cl_id = hif->hif_classid++;
 497         cl->cl_handle = qid;
 498         cl->cl_hif = hif;
 499         cl->cl_parent = parent;
 500
 501 #ifdef __NetBSD__
 502         s = splnet();
 503 #else
 504         s = splimp();
 505 #endif
 506         IFQ_LOCK(hif->hif_ifq);
 507         hif->hif_classes++;
 508
 509         /*
 510          * find a free slot in the class table.  if the slot matching
 511          * the lower bits of qid is free, use this slot.  otherwise,
 512          * use the first free slot.
 513          */
 514         i = qid % HFSC_MAX_CLASSES;
 515         if (hif->hif_class_tbl[i] == NULL)
 516                 hif->hif_class_tbl[i] = cl;
 517         else {
 518                 for (i = 0; i < HFSC_MAX_CLASSES; i++)
 519                         if (hif->hif_class_tbl[i] == NULL) {
 520                                 hif->hif_class_tbl[i] = cl;
 521                                 break;
 522                         }
 523                 if (i == HFSC_MAX_CLASSES) {
 524                         IFQ_UNLOCK(hif->hif_ifq);
 525                         splx(s);
 526                         goto err_ret;
 527                 }
 528         }
 529
 530         if (flags & HFCF_DEFAULTCLASS)
 531                 hif->hif_defaultclass = cl;
 532
 533         if (parent == NULL) {
 534                 /* this is root class */
 535                 hif->hif_rootclass = cl;
 536         } else {
 537                 /* add this class to the children list of the parent */
 538                 if ((p = parent->cl_children) == NULL)
 539                         parent->cl_children = cl;
 540                 else {
 541                         while (p->cl_siblings != NULL)
 542                                 p = p->cl_siblings;
 543                         p->cl_siblings = cl;
 544                 }
 545         }
 546         IFQ_UNLOCK(hif->hif_ifq);
 547         splx(s);
 548
 549         return (cl);
 550
 551  err_ret:
 552         if (cl->cl_actc != NULL)
 553                 actlist_destroy(cl->cl_actc);
 554         if (cl->cl_red != NULL) {
 555 #ifdef ALTQ_RIO
 556                 if (q_is_rio(cl->cl_q))
 557                         rio_destroy((rio_t *)cl->cl_red);
 558 #endif
 559 #ifdef ALTQ_RED
 560                 if (q_is_red(cl->cl_q))
 561                         red_destroy(cl->cl_red);
 562 #endif
 563         }
 564         if (cl->cl_fsc != NULL)
 565                 free(cl->cl_fsc, M_DEVBUF);
 566         if (cl->cl_rsc != NULL)
 567                 free(cl->cl_rsc, M_DEVBUF);
 568         if (cl->cl_usc != NULL)
 569                 free(cl->cl_usc, M_DEVBUF);
 570         if (cl->cl_q != NULL)
 571                 free(cl->cl_q, M_DEVBUF);
 572         free(cl, M_DEVBUF);
 573         return (NULL);
 574 }
 575
 576 static int
 577 hfsc_class_destroy(struct hfsc_class *cl)
 578 {
 579         int i, s;
 580
 581         if (cl == NULL)
 582                 return (0);
 583
 584         if (is_a_parent_class(cl))
 585                 return (EBUSY);
 586
 587 #ifdef __NetBSD__
 588         s = splnet();
 589 #else
 590         s = splimp();
 591 #endif
 592         IFQ_LOCK(cl->cl_hif->hif_ifq);
 593
 594 #ifdef ALTQ3_COMPAT
 595         /* delete filters referencing to this class */
 596         acc_discard_filters(&cl->cl_hif->hif_classifier, cl, 0);
 597 #endif /* ALTQ3_COMPAT */
 598
 599         if (!qempty(cl->cl_q))
 600                 hfsc_purgeq(cl);
 601
 602         if (cl->cl_parent == NULL) {
 603                 /* this is root class */
 604         } else {
 605                 struct hfsc_class *p = cl->cl_parent->cl_children;
 606
 607                 if (p == cl)
 608                         cl->cl_parent->cl_children = cl->cl_siblings;
 609                 else do {
 610                         if (p->cl_siblings == cl) {
 611                                 p->cl_siblings = cl->cl_siblings;
 612                                 break;
 613                         }
 614                 } while ((p = p->cl_siblings) != NULL);
 615                 ASSERT(p != NULL);
 616         }
 617
 618         for (i = 0; i < HFSC_MAX_CLASSES; i++)
 619                 if (cl->cl_hif->hif_class_tbl[i] == cl) {
 620                         cl->cl_hif->hif_class_tbl[i] = NULL;
 621                         break;
 622                 }
 623
 624         cl->cl_hif->hif_classes--;
 625         IFQ_UNLOCK(cl->cl_hif->hif_ifq);
 626         splx(s);
 627
 628         actlist_destroy(cl->cl_actc);
 629
 630         if (cl->cl_red != NULL) {
 631 #ifdef ALTQ_RIO
 632                 if (q_is_rio(cl->cl_q))
 633                         rio_destroy((rio_t *)cl->cl_red);
 634 #endif
 635 #ifdef ALTQ_RED
 636                 if (q_is_red(cl->cl_q))
 637                         red_destroy(cl->cl_red);
 638 #endif
 639         }
 640
 641         IFQ_LOCK(cl->cl_hif->hif_ifq);
 642         if (cl == cl->cl_hif->hif_rootclass)
 643                 cl->cl_hif->hif_rootclass = NULL;
 644         if (cl == cl->cl_hif->hif_defaultclass)
 645                 cl->cl_hif->hif_defaultclass = NULL;
 646         IFQ_UNLOCK(cl->cl_hif->hif_ifq);
 647
 648         if (cl->cl_usc != NULL)
 649                 free(cl->cl_usc, M_DEVBUF);
 650         if (cl->cl_fsc != NULL)
 651                 free(cl->cl_fsc, M_DEVBUF);
 652         if (cl->cl_rsc != NULL)
 653                 free(cl->cl_rsc, M_DEVBUF);
 654         free(cl->cl_q, M_DEVBUF);
 655         free(cl, M_DEVBUF);
 656
 657         return (0);
 658 }
 659
 660 /*
 661  * hfsc_nextclass returns the next class in the tree.
 662  *   usage:
 663  *      for (cl = hif->hif_rootclass; cl != NULL; cl = hfsc_nextclass(cl))
 664  *              do_something;
 665  */
 666 static struct hfsc_class *
 667 hfsc_nextclass(struct hfsc_class *cl)
 668 {
 669         if (cl->cl_children != NULL)
 670                 cl = cl->cl_children;
 671         else if (cl->cl_siblings != NULL)
 672                 cl = cl->cl_siblings;
 673         else {
 674                 while ((cl = cl->cl_parent) != NULL)
 675                         if (cl->cl_siblings) {
 676                                 cl = cl->cl_siblings;
 677                                 break;
 678                         }
 679         }
 680
 681         return (cl);
 682 }
 683
 684 /*
 685  * hfsc_enqueue is an enqueue function to be registered to
 686  * (*altq_enqueue) in struct ifaltq.
 687  */
 688 static int
 689 hfsc_enqueue(struct ifaltq *ifq, struct mbuf *m, struct altq_pktattr *pktattr)
 690 {
 691         struct hfsc_if  *hif = (struct hfsc_if *)ifq->altq_disc;
 692         struct hfsc_class *cl;
 693         struct pf_mtag *t;
 694         int len;
 695
 696         IFQ_LOCK_ASSERT(ifq);
 697
 698         /* grab class set by classifier */
 699         if ((m->m_flags & M_PKTHDR) == 0) {
 700                 /* should not happen */
 701 #if defined(__NetBSD__) || defined(__OpenBSD__)\
 702     || (defined(__FreeBSD__) && __FreeBSD_version >= 501113)
 703                 printf("altq: packet for %s does not have pkthdr\n",
 704                     ifq->altq_ifp->if_xname);
 705 #else
 706                 printf("altq: packet for %s%d does not have pkthdr\n",
 707                     ifq->altq_ifp->if_name, ifq->altq_ifp->if_unit);
 708 #endif
 709                 m_freem(m);
 710                 return (ENOBUFS);
 711         }
 712         cl = NULL;
 713         if ((t = pf_find_mtag(m)) != NULL)
 714                 cl = clh_to_clp(hif, t->qid);
 715 #ifdef ALTQ3_COMPAT
 716         else if ((ifq->altq_flags & ALTQF_CLASSIFY) && pktattr != NULL)
 717                 cl = pktattr->pattr_class;
 718 #endif
 719         if (cl == NULL || is_a_parent_class(cl)) {
 720                 cl = hif->hif_defaultclass;
 721                 if (cl == NULL) {
 722                         m_freem(m);
 723                         return (ENOBUFS);
 724                 }
 725         }
 726 #ifdef ALTQ3_COMPAT
 727         if (pktattr != NULL)
 728                 cl->cl_pktattr = pktattr;  /* save proto hdr used by ECN */
 729         else
 730 #endif
 731                 cl->cl_pktattr = NULL;
 732         len = m_pktlen(m);
 733         if (hfsc_addq(cl, m) != 0) {
 734                 /* drop occurred.  mbuf was freed in hfsc_addq. */
 735                 PKTCNTR_ADD(&cl->cl_stats.drop_cnt, len);
 736                 return (ENOBUFS);
 737         }
 738         IFQ_INC_LEN(ifq);
 739         cl->cl_hif->hif_packets++;
 740
 741         /* successfully queued. */
 742         if (qlen(cl->cl_q) == 1)
 743                 set_active(cl, m_pktlen(m));
 744
 745         return (0);
 746 }
 747
 748 /*
 749  * hfsc_dequeue is a dequeue function to be registered to
 750  * (*altq_dequeue) in struct ifaltq.
 751  *
 752  * note: ALTDQ_POLL returns the next packet without removing the packet
 753  *      from the queue.  ALTDQ_REMOVE is a normal dequeue operation.
 754  *      ALTDQ_REMOVE must return the same packet if called immediately
 755  *      after ALTDQ_POLL.
 756  */
 757 static struct mbuf *
 758 hfsc_dequeue(struct ifaltq *ifq, int op)
 759 {
 760         struct hfsc_if  *hif = (struct hfsc_if *)ifq->altq_disc;
 761         struct hfsc_class *cl;
 762         struct mbuf *m;
 763         int len, next_len;
 764         int realtime = 0;
 765         u_int64_t cur_time;
 766
 767         IFQ_LOCK_ASSERT(ifq);
 768
 769         if (hif->hif_packets == 0)
 770                 /* no packet in the tree */
 771                 return (NULL);
 772
 773         cur_time = read_machclk();
 774
 775         if (op == ALTDQ_REMOVE && hif->hif_pollcache != NULL) {
 776
 777                 cl = hif->hif_pollcache;
 778                 hif->hif_pollcache = NULL;
 779                 /* check if the class was scheduled by real-time criteria */
 780                 if (cl->cl_rsc != NULL)
 781                         realtime = (cl->cl_e <= cur_time);
 782         } else {
 783                 /*
 784                  * if there are eligible classes, use real-time criteria.
 785                  * find the class with the minimum deadline among
 786                  * the eligible classes.
 787                  */
 788                 if ((cl = ellist_get_mindl(hif->hif_eligible, cur_time))
 789                     != NULL) {
 790                         realtime = 1;
 791                 } else {
 792 #ifdef ALTQ_DEBUG
 793                         int fits = 0;
 794 #endif
 795                         /*
 796                          * use link-sharing criteria
 797                          * get the class with the minimum vt in the hierarchy
 798                          */
 799                         cl = hif->hif_rootclass;
 800                         while (is_a_parent_class(cl)) {
 801
 802                                 cl = actlist_firstfit(cl, cur_time);
 803                                 if (cl == NULL) {
 804 #ifdef ALTQ_DEBUG
 805                                         if (fits > 0)
 806                                                 printf("%d fit but none found\n",fits);
 807 #endif
 808                                         return (NULL);
 809                                 }
 810                                 /*
 811                                  * update parent's cl_cvtmin.
 812                                  * don't update if the new vt is smaller.
 813                                  */
 814                                 if (cl->cl_parent->cl_cvtmin < cl->cl_vt)
 815                                         cl->cl_parent->cl_cvtmin = cl->cl_vt;
 816 #ifdef ALTQ_DEBUG
 817                                 fits++;
 818 #endif
 819                         }
 820                 }
 821
 822                 if (op == ALTDQ_POLL) {
 823                         hif->hif_pollcache = cl;
 824                         m = hfsc_pollq(cl);
 825                         return (m);
 826                 }
 827         }
 828
 829         m = hfsc_getq(cl);
 830         if (m == NULL)
 831                 panic("hfsc_dequeue:");
 832         len = m_pktlen(m);
 833         cl->cl_hif->hif_packets--;
 834         IFQ_DEC_LEN(ifq);
 835         PKTCNTR_ADD(&cl->cl_stats.xmit_cnt, len);
 836
 837         update_vf(cl, len, cur_time);
 838         if (realtime)
 839                 cl->cl_cumul += len;
 840
 841         if (!qempty(cl->cl_q)) {
 842                 if (cl->cl_rsc != NULL) {
 843                         /* update ed */
 844                         next_len = m_pktlen(qhead(cl->cl_q));
 845
 846                         if (realtime)
 847                                 update_ed(cl, next_len);
 848                         else
 849                                 update_d(cl, next_len);
 850                 }
 851         } else {
 852                 /* the class becomes passive */
 853                 set_passive(cl);
 854         }
 855
 856         return (m);
 857 }
 858
 859 static int
 860 hfsc_addq(struct hfsc_class *cl, struct mbuf *m)
 861 {
 862
 863 #ifdef ALTQ_RIO
 864         if (q_is_rio(cl->cl_q))
 865                 return rio_addq((rio_t *)cl->cl_red, cl->cl_q,
 866                                 m, cl->cl_pktattr);
 867 #endif
 868 #ifdef ALTQ_RED
 869         if (q_is_red(cl->cl_q))
 870                 return red_addq(cl->cl_red, cl->cl_q, m, cl->cl_pktattr);
 871 #endif
 872         if (qlen(cl->cl_q) >= qlimit(cl->cl_q)) {
 873                 m_freem(m);
 874                 return (-1);
 875         }
 876
 877         if (cl->cl_flags & HFCF_CLEARDSCP)
 878                 write_dsfield(m, cl->cl_pktattr, 0);
 879
 880         _addq(cl->cl_q, m);
 881
 882         return (0);
 883 }
 884
 885 static struct mbuf *
 886 hfsc_getq(struct hfsc_class *cl)
 887 {
 888 #ifdef ALTQ_RIO
 889         if (q_is_rio(cl->cl_q))
 890                 return rio_getq((rio_t *)cl->cl_red, cl->cl_q);
 891 #endif
 892 #ifdef ALTQ_RED
 893         if (q_is_red(cl->cl_q))
 894                 return red_getq(cl->cl_red, cl->cl_q);
 895 #endif
 896         return _getq(cl->cl_q);
 897 }
 898
 899 static struct mbuf *
 900 hfsc_pollq(struct hfsc_class *cl)
 901 {
 902         return qhead(cl->cl_q);
 903 }
 904
 905 static void
 906 hfsc_purgeq(struct hfsc_class *cl)
 907 {
 908         struct mbuf *m;
 909
 910         if (qempty(cl->cl_q))
 911                 return;
 912
 913         while ((m = _getq(cl->cl_q)) != NULL) {
 914                 PKTCNTR_ADD(&cl->cl_stats.drop_cnt, m_pktlen(m));
 915                 m_freem(m);
 916                 cl->cl_hif->hif_packets--;
 917                 IFQ_DEC_LEN(cl->cl_hif->hif_ifq);
 918         }
 919         ASSERT(qlen(cl->cl_q) == 0);
 920
 921         update_vf(cl, 0, 0);    /* remove cl from the actlist */
 922         set_passive(cl);
 923 }
 924
 925 static void
 926 set_active(struct hfsc_class *cl, int len)
 927 {
 928         if (cl->cl_rsc != NULL)
 929                 init_ed(cl, len);
 930         if (cl->cl_fsc != NULL)
 931                 init_vf(cl, len);
 932
 933         cl->cl_stats.period++;
 934 }
 935
 936 static void
 937 set_passive(struct hfsc_class *cl)
 938 {
 939         if (cl->cl_rsc != NULL)
 940                 ellist_remove(cl);
 941
 942         /*
 943          * actlist is now handled in update_vf() so that update_vf(cl, 0, 0)
 944          * needs to be called explicitly to remove a class from actlist
 945          */
 946 }
 947
 948 static void
 949 init_ed(struct hfsc_class *cl, int next_len)
 950 {
 951         u_int64_t cur_time;
 952
 953         cur_time = read_machclk();
 954
 955         /* update the deadline curve */
 956         rtsc_min(&cl->cl_deadline, cl->cl_rsc, cur_time, cl->cl_cumul);
 957
 958         /*
 959          * update the eligible curve.
 960          * for concave, it is equal to the deadline curve.
 961          * for convex, it is a linear curve with slope m2.
 962          */
 963         cl->cl_eligible = cl->cl_deadline;
 964         if (cl->cl_rsc->sm1 <= cl->cl_rsc->sm2) {
 965                 cl->cl_eligible.dx = 0;
 966                 cl->cl_eligible.dy = 0;
 967         }
 968
 969         /* compute e and d */
 970         cl->cl_e = rtsc_y2x(&cl->cl_eligible, cl->cl_cumul);
 971         cl->cl_d = rtsc_y2x(&cl->cl_deadline, cl->cl_cumul + next_len);
 972
 973         ellist_insert(cl);
 974 }
 975
 976 static void
 977 update_ed(struct hfsc_class *cl, int next_len)
 978 {
 979         cl->cl_e = rtsc_y2x(&cl->cl_eligible, cl->cl_cumul);
 980         cl->cl_d = rtsc_y2x(&cl->cl_deadline, cl->cl_cumul + next_len);
 981
 982         ellist_update(cl);
 983 }
 984
 985 static void
 986 update_d(struct hfsc_class *cl, int next_len)
 987 {
 988         cl->cl_d = rtsc_y2x(&cl->cl_deadline, cl->cl_cumul + next_len);
 989 }
 990
 991 static void
 992 init_vf(struct hfsc_class *cl, int len)
 993 {
 994         struct hfsc_class *max_cl, *p;
 995         u_int64_t vt, f, cur_time;
 996         int go_active;
 997
 998         cur_time = 0;
 999         go_active = 1;
1000         for ( ; cl->cl_parent != NULL; cl = cl->cl_parent) {
1001
1002                 if (go_active && cl->cl_nactive++ == 0)
1003                         go_active = 1;
1004                 else
1005                         go_active = 0;
1006
1007                 if (go_active) {
1008                         max_cl = actlist_last(cl->cl_parent->cl_actc);
1009                         if (max_cl != NULL) {
1010                                 /*
1011                                  * set vt to the average of the min and max
1012                                  * classes.  if the parent's period didn't
1013                                  * change, don't decrease vt of the class.
1014                                  */
1015                                 vt = max_cl->cl_vt;
1016                                 if (cl->cl_parent->cl_cvtmin != 0)
1017                                         vt = (cl->cl_parent->cl_cvtmin + vt)/2;
1018
1019                                 if (cl->cl_parent->cl_vtperiod !=
1020                                     cl->cl_parentperiod || vt > cl->cl_vt)
1021                                         cl->cl_vt = vt;
1022                         } else {
1023                                 /*
1024                                  * first child for a new parent backlog period.
1025                                  * add parent's cvtmax to vtoff of children
1026                                  * to make a new vt (vtoff + vt) larger than
1027                                  * the vt in the last period for all children.
1028                                  */
1029                                 vt = cl->cl_parent->cl_cvtmax;
1030                                 for (p = cl->cl_parent->cl_children; p != NULL;
1031                                      p = p->cl_siblings)
1032                                         p->cl_vtoff += vt;
1033                                 cl->cl_vt = 0;
1034                                 cl->cl_parent->cl_cvtmax = 0;
1035                                 cl->cl_parent->cl_cvtmin = 0;
1036                         }
1037                         cl->cl_initvt = cl->cl_vt;
1038
1039                         /* update the virtual curve */
1040                         vt = cl->cl_vt + cl->cl_vtoff;
1041                         rtsc_min(&cl->cl_virtual, cl->cl_fsc, vt, cl->cl_total);
1042                         if (cl->cl_virtual.x == vt) {
1043                                 cl->cl_virtual.x -= cl->cl_vtoff;
1044                                 cl->cl_vtoff = 0;
1045                         }
1046                         cl->cl_vtadj = 0;
1047
1048                         cl->cl_vtperiod++;  /* increment vt period */
1049                         cl->cl_parentperiod = cl->cl_parent->cl_vtperiod;
1050                         if (cl->cl_parent->cl_nactive == 0)
1051                                 cl->cl_parentperiod++;
1052                         cl->cl_f = 0;
1053
1054                         actlist_insert(cl);
1055
1056                         if (cl->cl_usc != NULL) {
1057                                 /* class has upper limit curve */
1058                                 if (cur_time == 0)
1059                                         cur_time = read_machclk();
1060
1061                                 /* update the ulimit curve */
1062                                 rtsc_min(&cl->cl_ulimit, cl->cl_usc, cur_time,
1063                                     cl->cl_total);
1064                                 /* compute myf */
1065                                 cl->cl_myf = rtsc_y2x(&cl->cl_ulimit,
1066                                     cl->cl_total);
1067                                 cl->cl_myfadj = 0;
1068                         }
1069                 }
1070
1071                 if (cl->cl_myf > cl->cl_cfmin)
1072                         f = cl->cl_myf;
1073                 else
1074                         f = cl->cl_cfmin;
1075                 if (f != cl->cl_f) {
1076                         cl->cl_f = f;
1077                         update_cfmin(cl->cl_parent);
1078                 }
1079         }
1080 }
1081
1082 static void
1083 update_vf(struct hfsc_class *cl, int len, u_int64_t cur_time)
1084 {
1085         u_int64_t f, myf_bound, delta;
1086         int go_passive;
1087
1088         go_passive = qempty(cl->cl_q);
1089
1090         for (; cl->cl_parent != NULL; cl = cl->cl_parent) {
1091
1092                 cl->cl_total += len;
1093
1094                 if (cl->cl_fsc == NULL || cl->cl_nactive == 0)
1095                         continue;
1096
1097                 if (go_passive && --cl->cl_nactive == 0)
1098                         go_passive = 1;
1099                 else
1100                         go_passive = 0;
1101
1102                 if (go_passive) {
1103                         /* no more active child, going passive */
1104
1105                         /* update cvtmax of the parent class */
1106                         if (cl->cl_vt > cl->cl_parent->cl_cvtmax)
1107                                 cl->cl_parent->cl_cvtmax = cl->cl_vt;
1108
1109                         /* remove this class from the vt list */
1110                         actlist_remove(cl);
1111
1112                         update_cfmin(cl->cl_parent);
1113
1114                         continue;
1115                 }
1116
1117                 /*
1118                  * update vt and f
1119                  */
1120                 cl->cl_vt = rtsc_y2x(&cl->cl_virtual, cl->cl_total)
1121                     - cl->cl_vtoff + cl->cl_vtadj;
1122
1123                 /*
1124                  * if vt of the class is smaller than cvtmin,
1125                  * the class was skipped in the past due to non-fit.
1126                  * if so, we need to adjust vtadj.
1127                  */
1128                 if (cl->cl_vt < cl->cl_parent->cl_cvtmin) {
1129                         cl->cl_vtadj += cl->cl_parent->cl_cvtmin - cl->cl_vt;
1130                         cl->cl_vt = cl->cl_parent->cl_cvtmin;
1131                 }
1132
1133                 /* update the vt list */
1134                 actlist_update(cl);
1135
1136                 if (cl->cl_usc != NULL) {
1137                         cl->cl_myf = cl->cl_myfadj
1138                             + rtsc_y2x(&cl->cl_ulimit, cl->cl_total);
1139
1140                         /*
1141                          * if myf lags behind by more than one clock tick
1142                          * from the current time, adjust myfadj to prevent
1143                          * a rate-limited class from going greedy.
1144                          * in a steady state under rate-limiting, myf
1145                          * fluctuates within one clock tick.
1146                          */
1147                         myf_bound = cur_time - machclk_per_tick;
1148                         if (cl->cl_myf < myf_bound) {
1149                                 delta = cur_time - cl->cl_myf;
1150                                 cl->cl_myfadj += delta;
1151                                 cl->cl_myf += delta;
1152                         }
1153                 }
1154
1155                 /* cl_f is max(cl_myf, cl_cfmin) */
1156                 if (cl->cl_myf > cl->cl_cfmin)
1157                         f = cl->cl_myf;
1158                 else
1159                         f = cl->cl_cfmin;
1160                 if (f != cl->cl_f) {
1161                         cl->cl_f = f;
1162                         update_cfmin(cl->cl_parent);
1163                 }
1164         }
1165 }
1166
1167 static void
1168 update_cfmin(struct hfsc_class *cl)
1169 {
1170         struct hfsc_class *p;
1171         u_int64_t cfmin;
1172
1173         if (TAILQ_EMPTY(cl->cl_actc)) {
1174                 cl->cl_cfmin = 0;
1175                 return;
1176         }
1177         cfmin = HT_INFINITY;
1178         TAILQ_FOREACH(p, cl->cl_actc, cl_actlist) {
1179                 if (p->cl_f == 0) {
1180                         cl->cl_cfmin = 0;
1181                         return;
1182                 }
1183                 if (p->cl_f < cfmin)
1184                         cfmin = p->cl_f;
1185         }
1186         cl->cl_cfmin = cfmin;
1187 }
1188
1189 /*
1190  * TAILQ based ellist and actlist implementation
1191  * (ion wanted to make a calendar queue based implementation)
1192  */
1193 /*
1194  * eligible list holds backlogged classes being sorted by their eligible times.
1195  * there is one eligible list per interface.
1196  */
1197
1198 static ellist_t *
1199 ellist_alloc(void)
1200 {
1201         ellist_t *head;
1202
1203         head = malloc(sizeof(ellist_t), M_DEVBUF, M_WAITOK);
1204         TAILQ_INIT(head);
1205         return (head);
1206 }
1207
1208 static void
1209 ellist_destroy(ellist_t *head)
1210 {
1211         free(head, M_DEVBUF);
1212 }
1213
1214 static void
1215 ellist_insert(struct hfsc_class *cl)
1216 {
1217         struct hfsc_if  *hif = cl->cl_hif;
1218         struct hfsc_class *p;
1219
1220         /* check the last entry first */
1221         if ((p = TAILQ_LAST(hif->hif_eligible, _eligible)) == NULL ||
1222             p->cl_e <= cl->cl_e) {
1223                 TAILQ_INSERT_TAIL(hif->hif_eligible, cl, cl_ellist);
1224                 return;
1225         }
1226
1227         TAILQ_FOREACH(p, hif->hif_eligible, cl_ellist) {
1228                 if (cl->cl_e < p->cl_e) {
1229                         TAILQ_INSERT_BEFORE(p, cl, cl_ellist);
1230                         return;
1231                 }
1232         }
1233         ASSERT(0); /* should not reach here */
1234 }
1235
1236 static void
1237 ellist_remove(struct hfsc_class *cl)
1238 {
1239         struct hfsc_if  *hif = cl->cl_hif;
1240
1241         TAILQ_REMOVE(hif->hif_eligible, cl, cl_ellist);
1242 }
1243
1244 static void
1245 ellist_update(struct hfsc_class *cl)
1246 {
1247         struct hfsc_if  *hif = cl->cl_hif;
1248         struct hfsc_class *p, *last;
1249
1250         /*
1251          * the eligible time of a class increases monotonically.
1252          * if the next entry has a larger eligible time, nothing to do.
1253          */
1254         p = TAILQ_NEXT(cl, cl_ellist);
1255         if (p == NULL || cl->cl_e <= p->cl_e)
1256                 return;
1257
1258         /* check the last entry */
1259         last = TAILQ_LAST(hif->hif_eligible, _eligible);
1260         ASSERT(last != NULL);
1261         if (last->cl_e <= cl->cl_e) {
1262                 TAILQ_REMOVE(hif->hif_eligible, cl, cl_ellist);
1263                 TAILQ_INSERT_TAIL(hif->hif_eligible, cl, cl_ellist);
1264                 return;
1265         }
1266
1267         /*
1268          * the new position must be between the next entry
1269          * and the last entry
1270          */
1271         while ((p = TAILQ_NEXT(p, cl_ellist)) != NULL) {
1272                 if (cl->cl_e < p->cl_e) {
1273                         TAILQ_REMOVE(hif->hif_eligible, cl, cl_ellist);
1274                         TAILQ_INSERT_BEFORE(p, cl, cl_ellist);
1275                         return;
1276                 }
1277         }
1278         ASSERT(0); /* should not reach here */
1279 }
1280
1281 /* find the class with the minimum deadline among the eligible classes */
1282 struct hfsc_class *
1283 ellist_get_mindl(ellist_t *head, u_int64_t cur_time)
1284 {
1285         struct hfsc_class *p, *cl = NULL;
1286
1287         TAILQ_FOREACH(p, head, cl_ellist) {
1288                 if (p->cl_e > cur_time)
1289                         break;
1290                 if (cl == NULL || p->cl_d < cl->cl_d)
1291                         cl = p;
1292         }
1293         return (cl);
1294 }
1295
1296 /*
1297  * active children list holds backlogged child classes being sorted
1298  * by their virtual time.
1299  * each intermediate class has one active children list.
1300  */
1301 static actlist_t *
1302 actlist_alloc(void)
1303 {
1304         actlist_t *head;
1305
1306         head = malloc(sizeof(actlist_t), M_DEVBUF, M_WAITOK);
1307         TAILQ_INIT(head);
1308         return (head);
1309 }
1310
1311 static void
1312 actlist_destroy(actlist_t *head)
1313 {
1314         free(head, M_DEVBUF);
1315 }
1316 static void
1317 actlist_insert(struct hfsc_class *cl)
1318 {
1319         struct hfsc_class *p;
1320
1321         /* check the last entry first */
1322         if ((p = TAILQ_LAST(cl->cl_parent->cl_actc, _active)) == NULL
1323             || p->cl_vt <= cl->cl_vt) {
1324                 TAILQ_INSERT_TAIL(cl->cl_parent->cl_actc, cl, cl_actlist);
1325                 return;
1326         }
1327
1328         TAILQ_FOREACH(p, cl->cl_parent->cl_actc, cl_actlist) {
1329                 if (cl->cl_vt < p->cl_vt) {
1330                         TAILQ_INSERT_BEFORE(p, cl, cl_actlist);
1331                         return;
1332                 }
1333         }
1334         ASSERT(0); /* should not reach here */
1335 }
1336
1337 static void
1338 actlist_remove(struct hfsc_class *cl)
1339 {
1340         TAILQ_REMOVE(cl->cl_parent->cl_actc, cl, cl_actlist);
1341 }
1342
1343 static void
1344 actlist_update(struct hfsc_class *cl)
1345 {
1346         struct hfsc_class *p, *last;
1347
1348         /*
1349          * the virtual time of a class increases monotonically during its
1350          * backlogged period.
1351          * if the next entry has a larger virtual time, nothing to do.
1352          */
1353         p = TAILQ_NEXT(cl, cl_actlist);
1354         if (p == NULL || cl->cl_vt < p->cl_vt)
1355                 return;
1356
1357         /* check the last entry */
1358         last = TAILQ_LAST(cl->cl_parent->cl_actc, _active);
1359         ASSERT(last != NULL);
1360         if (last->cl_vt <= cl->cl_vt) {
1361                 TAILQ_REMOVE(cl->cl_parent->cl_actc, cl, cl_actlist);
1362                 TAILQ_INSERT_TAIL(cl->cl_parent->cl_actc, cl, cl_actlist);
1363                 return;
1364         }
1365
1366         /*
1367          * the new position must be between the next entry
1368          * and the last entry
1369          */
1370         while ((p = TAILQ_NEXT(p, cl_actlist)) != NULL) {
1371                 if (cl->cl_vt < p->cl_vt) {
1372                         TAILQ_REMOVE(cl->cl_parent->cl_actc, cl, cl_actlist);
1373                         TAILQ_INSERT_BEFORE(p, cl, cl_actlist);
1374                         return;
1375                 }
1376         }
1377         ASSERT(0); /* should not reach here */
1378 }
1379
1380 static struct hfsc_class *
1381 actlist_firstfit(struct hfsc_class *cl, u_int64_t cur_time)
1382 {
1383         struct hfsc_class *p;
1384
1385         TAILQ_FOREACH(p, cl->cl_actc, cl_actlist) {
1386                 if (p->cl_f <= cur_time)
1387                         return (p);
1388         }
1389         return (NULL);
1390 }
1391
1392 /*
1393  * service curve support functions
1394  *
1395  *  external service curve parameters
1396  *      m: bits/sec
1397  *      d: msec
1398  *  internal service curve parameters
1399  *      sm: (bytes/tsc_interval) << SM_SHIFT
1400  *      ism: (tsc_count/byte) << ISM_SHIFT
1401  *      dx: tsc_count
1402  *
1403  * SM_SHIFT and ISM_SHIFT are scaled in order to keep effective digits.
1404  * we should be able to handle 100K-1Gbps linkspeed with 200Hz-1GHz CPU
1405  * speed.  SM_SHIFT and ISM_SHIFT are selected to have at least 3 effective
1406  * digits in decimal using the following table.
1407  *
1408  *  bits/sec    100Kbps     1Mbps     10Mbps     100Mbps    1Gbps
1409  *  ----------+-------------------------------------------------------
1410  *  bytes/nsec  12.5e-6    125e-6     1250e-6    12500e-6   125000e-6
1411  *  sm(500MHz)  25.0e-6    250e-6     2500e-6    25000e-6   250000e-6
1412  *  sm(200MHz)  62.5e-6    625e-6     6250e-6    62500e-6   625000e-6
1413  *
1414  *  nsec/byte   80000      8000       800        80         8
1415  *  ism(500MHz) 40000      4000       400        40         4
1416  *  ism(200MHz) 16000      1600       160        16         1.6
1417  */
1418 #define SM_SHIFT        24
1419 #define ISM_SHIFT       10
1420
1421 #define SM_MASK         ((1LL << SM_SHIFT) - 1)
1422 #define ISM_MASK        ((1LL << ISM_SHIFT) - 1)
1423
1424 static __inline u_int64_t
1425 seg_x2y(u_int64_t x, u_int64_t sm)
1426 {
1427         u_int64_t y;
1428
1429         /*
1430          * compute
1431          *      y = x * sm >> SM_SHIFT
1432          * but divide it for the upper and lower bits to avoid overflow
1433          */
1434         y = (x >> SM_SHIFT) * sm + (((x & SM_MASK) * sm) >> SM_SHIFT);
1435         return (y);
1436 }
1437
1438 static __inline u_int64_t
1439 seg_y2x(u_int64_t y, u_int64_t ism)
1440 {
1441         u_int64_t x;
1442
1443         if (y == 0)
1444                 x = 0;
1445         else if (ism == HT_INFINITY)
1446                 x = HT_INFINITY;
1447         else {
1448                 x = (y >> ISM_SHIFT) * ism
1449                     + (((y & ISM_MASK) * ism) >> ISM_SHIFT);
1450         }
1451         return (x);
1452 }
1453
1454 static __inline u_int64_t
1455 m2sm(u_int m)
1456 {
1457         u_int64_t sm;
1458
1459         sm = ((u_int64_t)m << SM_SHIFT) / 8 / machclk_freq;
1460         return (sm);
1461 }
1462
1463 static __inline u_int64_t
1464 m2ism(u_int m)
1465 {
1466         u_int64_t ism;
1467
1468         if (m == 0)
1469                 ism = HT_INFINITY;
1470         else
1471                 ism = ((u_int64_t)machclk_freq << ISM_SHIFT) * 8 / m;
1472         return (ism);
1473 }
1474
1475 static __inline u_int64_t
1476 d2dx(u_int d)
1477 {
1478         u_int64_t dx;
1479
1480         dx = ((u_int64_t)d * machclk_freq) / 1000;
1481         return (dx);
1482 }
1483
1484 static u_int
1485 sm2m(u_int64_t sm)
1486 {
1487         u_int64_t m;
1488
1489         m = (sm * 8 * machclk_freq) >> SM_SHIFT;
1490         return ((u_int)m);
1491 }
1492
1493 static u_int
1494 dx2d(u_int64_t dx)
1495 {
1496         u_int64_t d;
1497
1498         d = dx * 1000 / machclk_freq;
1499         return ((u_int)d);
1500 }
1501
1502 static void
1503 sc2isc(struct service_curve *sc, struct internal_sc *isc)
1504 {
1505         isc->sm1 = m2sm(sc->m1);
1506         isc->ism1 = m2ism(sc->m1);
1507         isc->dx = d2dx(sc->d);
1508         isc->dy = seg_x2y(isc->dx, isc->sm1);
1509         isc->sm2 = m2sm(sc->m2);
1510         isc->ism2 = m2ism(sc->m2);
1511 }
1512
1513 /*
1514  * initialize the runtime service curve with the given internal
1515  * service curve starting at (x, y).
1516  */
1517 static void
1518 rtsc_init(struct runtime_sc *rtsc, struct internal_sc * isc, u_int64_t x,
1519     u_int64_t y)
1520 {
1521         rtsc->x =       x;
1522         rtsc->y =       y;
1523         rtsc->sm1 =     isc->sm1;
1524         rtsc->ism1 =    isc->ism1;
1525         rtsc->dx =      isc->dx;
1526         rtsc->dy =      isc->dy;
1527         rtsc->sm2 =     isc->sm2;
1528         rtsc->ism2 =    isc->ism2;
1529 }
1530
1531 /*
1532  * calculate the y-projection of the runtime service curve by the
1533  * given x-projection value
1534  */
1535 static u_int64_t
1536 rtsc_y2x(struct runtime_sc *rtsc, u_int64_t y)
1537 {
1538         u_int64_t       x;
1539
1540         if (y < rtsc->y)
1541                 x = rtsc->x;
1542         else if (y <= rtsc->y + rtsc->dy) {
1543                 /* x belongs to the 1st segment */
1544                 if (rtsc->dy == 0)
1545                         x = rtsc->x + rtsc->dx;
1546                 else
1547                         x = rtsc->x + seg_y2x(y - rtsc->y, rtsc->ism1);
1548         } else {
1549                 /* x belongs to the 2nd segment */
1550                 x = rtsc->x + rtsc->dx
1551                     + seg_y2x(y - rtsc->y - rtsc->dy, rtsc->ism2);
1552         }
1553         return (x);
1554 }
1555
1556 static u_int64_t
1557 rtsc_x2y(struct runtime_sc *rtsc, u_int64_t x)
1558 {
1559         u_int64_t       y;
1560
1561         if (x <= rtsc->x)
1562                 y = rtsc->y;
1563         else if (x <= rtsc->x + rtsc->dx)
1564                 /* y belongs to the 1st segment */
1565                 y = rtsc->y + seg_x2y(x - rtsc->x, rtsc->sm1);
1566         else
1567                 /* y belongs to the 2nd segment */
1568                 y = rtsc->y + rtsc->dy
1569                     + seg_x2y(x - rtsc->x - rtsc->dx, rtsc->sm2);
1570         return (y);
1571 }
1572
1573 /*
1574  * update the runtime service curve by taking the minimum of the current
1575  * runtime service curve and the service curve starting at (x, y).
1576  */
1577 static void
1578 rtsc_min(struct runtime_sc *rtsc, struct internal_sc *isc, u_int64_t x,
1579     u_int64_t y)
1580 {
1581         u_int64_t       y1, y2, dx, dy;
1582
1583         if (isc->sm1 <= isc->sm2) {
1584                 /* service curve is convex */
1585                 y1 = rtsc_x2y(rtsc, x);
1586                 if (y1 < y)
1587                         /* the current rtsc is smaller */
1588                         return;
1589                 rtsc->x = x;
1590                 rtsc->y = y;
1591                 return;
1592         }
1593
1594         /*
1595          * service curve is concave
1596          * compute the two y values of the current rtsc
1597          *      y1: at x
1598          *      y2: at (x + dx)
1599          */
1600         y1 = rtsc_x2y(rtsc, x);
1601         if (y1 <= y) {
1602                 /* rtsc is below isc, no change to rtsc */
1603                 return;
1604         }
1605
1606         y2 = rtsc_x2y(rtsc, x + isc->dx);
1607         if (y2 >= y + isc->dy) {
1608                 /* rtsc is above isc, replace rtsc by isc */
1609                 rtsc->x = x;
1610                 rtsc->y = y;
1611                 rtsc->dx = isc->dx;
1612                 rtsc->dy = isc->dy;
1613                 return;
1614         }
1615
1616         /*
1617          * the two curves intersect
1618          * compute the offsets (dx, dy) using the reverse
1619          * function of seg_x2y()
1620          *      seg_x2y(dx, sm1) == seg_x2y(dx, sm2) + (y1 - y)
1621          */
1622         dx = ((y1 - y) << SM_SHIFT) / (isc->sm1 - isc->sm2);
1623         /*
1624          * check if (x, y1) belongs to the 1st segment of rtsc.
1625          * if so, add the offset.
1626          */
1627         if (rtsc->x + rtsc->dx > x)
1628                 dx += rtsc->x + rtsc->dx - x;
1629         dy = seg_x2y(dx, isc->sm1);
1630
1631         rtsc->x = x;
1632         rtsc->y = y;
1633         rtsc->dx = dx;
1634         rtsc->dy = dy;
1635         return;
1636 }
1637
1638 static void
1639 get_class_stats(struct hfsc_classstats *sp, struct hfsc_class *cl)
1640 {
1641         sp->class_id = cl->cl_id;
1642         sp->class_handle = cl->cl_handle;
1643
1644         if (cl->cl_rsc != NULL) {
1645                 sp->rsc.m1 = sm2m(cl->cl_rsc->sm1);
1646                 sp->rsc.d = dx2d(cl->cl_rsc->dx);
1647                 sp->rsc.m2 = sm2m(cl->cl_rsc->sm2);
1648         } else {
1649                 sp->rsc.m1 = 0;
1650                 sp->rsc.d = 0;
1651                 sp->rsc.m2 = 0;
1652         }
1653         if (cl->cl_fsc != NULL) {
1654                 sp->fsc.m1 = sm2m(cl->cl_fsc->sm1);
1655                 sp->fsc.d = dx2d(cl->cl_fsc->dx);
1656                 sp->fsc.m2 = sm2m(cl->cl_fsc->sm2);
1657         } else {
1658                 sp->fsc.m1 = 0;
1659                 sp->fsc.d = 0;
1660                 sp->fsc.m2 = 0;
1661         }
1662         if (cl->cl_usc != NULL) {
1663                 sp->usc.m1 = sm2m(cl->cl_usc->sm1);
1664                 sp->usc.d = dx2d(cl->cl_usc->dx);
1665                 sp->usc.m2 = sm2m(cl->cl_usc->sm2);
1666         } else {
1667                 sp->usc.m1 = 0;
1668                 sp->usc.d = 0;
1669                 sp->usc.m2 = 0;
1670         }
1671
1672         sp->total = cl->cl_total;
1673         sp->cumul = cl->cl_cumul;
1674
1675         sp->d = cl->cl_d;
1676         sp->e = cl->cl_e;
1677         sp->vt = cl->cl_vt;
1678         sp->f = cl->cl_f;
1679
1680         sp->initvt = cl->cl_initvt;
1681         sp->vtperiod = cl->cl_vtperiod;
1682         sp->parentperiod = cl->cl_parentperiod;
1683         sp->nactive = cl->cl_nactive;
1684         sp->vtoff = cl->cl_vtoff;
1685         sp->cvtmax = cl->cl_cvtmax;
1686         sp->myf = cl->cl_myf;
1687         sp->cfmin = cl->cl_cfmin;
1688         sp->cvtmin = cl->cl_cvtmin;
1689         sp->myfadj = cl->cl_myfadj;
1690         sp->vtadj = cl->cl_vtadj;
1691
1692         sp->cur_time = read_machclk();
1693         sp->machclk_freq = machclk_freq;
1694
1695         sp->qlength = qlen(cl->cl_q);
1696         sp->qlimit = qlimit(cl->cl_q);
1697         sp->xmit_cnt = cl->cl_stats.xmit_cnt;
1698         sp->drop_cnt = cl->cl_stats.drop_cnt;
1699         sp->period = cl->cl_stats.period;
1700
1701         sp->qtype = qtype(cl->cl_q);
1702 #ifdef ALTQ_RED
1703         if (q_is_red(cl->cl_q))
1704                 red_getstats(cl->cl_red, &sp->red[0]);
1705 #endif
1706 #ifdef ALTQ_RIO
1707         if (q_is_rio(cl->cl_q))
1708                 rio_getstats((rio_t *)cl->cl_red, &sp->red[0]);
1709 #endif
1710 }
1711
1712 /* convert a class handle to the corresponding class pointer */
1713 static struct hfsc_class *
1714 clh_to_clp(struct hfsc_if *hif, u_int32_t chandle)
1715 {
1716         int i;
1717         struct hfsc_class *cl;
1718
1719         if (chandle == 0)
1720                 return (NULL);
1721         /*
1722          * first, try optimistically the slot matching the lower bits of
1723          * the handle.  if it fails, do the linear table search.
1724          */
1725         i = chandle % HFSC_MAX_CLASSES;
1726         if ((cl = hif->hif_class_tbl[i]) != NULL && cl->cl_handle == chandle)
1727                 return (cl);
1728         for (i = 0; i < HFSC_MAX_CLASSES; i++)
1729                 if ((cl = hif->hif_class_tbl[i]) != NULL &&
1730                     cl->cl_handle == chandle)
1731                         return (cl);
1732         return (NULL);
1733 }
1734
1735 #ifdef ALTQ3_COMPAT
1736 static struct hfsc_if *
1737 hfsc_attach(ifq, bandwidth)
1738         struct ifaltq *ifq;
1739         u_int bandwidth;
1740 {
1741         struct hfsc_if *hif;
1742
1743         hif = malloc(sizeof(struct hfsc_if), M_DEVBUF, M_WAITOK);
1744         if (hif == NULL)
1745                 return (NULL);
1746         bzero(hif, sizeof(struct hfsc_if));
1747
1748         hif->hif_eligible = ellist_alloc();
1749         if (hif->hif_eligible == NULL) {
1750                 free(hif, M_DEVBUF);
1751                 return NULL;
1752         }
1753
1754         hif->hif_ifq = ifq;
1755
1756         /* add this state to the hfsc list */
1757         hif->hif_next = hif_list;
1758         hif_list = hif;
1759
1760         return (hif);
1761 }
1762
1763 static int
1764 hfsc_detach(hif)
1765         struct hfsc_if *hif;
1766 {
1767         (void)hfsc_clear_interface(hif);
1768         (void)hfsc_class_destroy(hif->hif_rootclass);
1769
1770         /* remove this interface from the hif list */
1771         if (hif_list == hif)
1772                 hif_list = hif->hif_next;
1773         else {
1774                 struct hfsc_if *h;
1775
1776                 for (h = hif_list; h != NULL; h = h->hif_next)
1777                         if (h->hif_next == hif) {
1778                                 h->hif_next = hif->hif_next;
1779                                 break;
1780                         }
1781                 ASSERT(h != NULL);
1782         }
1783
1784         ellist_destroy(hif->hif_eligible);
1785
1786         free(hif, M_DEVBUF);
1787
1788         return (0);
1789 }
1790
1791 static int
1792 hfsc_class_modify(cl, rsc, fsc, usc)
1793         struct hfsc_class *cl;
1794         struct service_curve *rsc, *fsc, *usc;
1795 {
1796         struct internal_sc *rsc_tmp, *fsc_tmp, *usc_tmp;
1797         u_int64_t cur_time;
1798         int s;
1799
1800         rsc_tmp = fsc_tmp = usc_tmp = NULL;
1801         if (rsc != NULL && (rsc->m1 != 0 || rsc->m2 != 0) &&
1802             cl->cl_rsc == NULL) {
1803                 rsc_tmp = malloc(sizeof(struct internal_sc),
1804                     M_DEVBUF, M_WAITOK);
1805                 if (rsc_tmp == NULL)
1806                         return (ENOMEM);
1807         }
1808         if (fsc != NULL && (fsc->m1 != 0 || fsc->m2 != 0) &&
1809             cl->cl_fsc == NULL) {
1810                 fsc_tmp = malloc(sizeof(struct internal_sc),
1811                     M_DEVBUF, M_WAITOK);
1812                 if (fsc_tmp == NULL) {
1813                         free(rsc_tmp);
1814                         return (ENOMEM);
1815                 }
1816         }
1817         if (usc != NULL && (usc->m1 != 0 || usc->m2 != 0) &&
1818             cl->cl_usc == NULL) {
1819                 usc_tmp = malloc(sizeof(struct internal_sc),
1820                     M_DEVBUF, M_WAITOK);
1821                 if (usc_tmp == NULL) {
1822                         free(rsc_tmp);
1823                         free(fsc_tmp);
1824                         return (ENOMEM);
1825                 }
1826         }
1827
1828         cur_time = read_machclk();
1829 #ifdef __NetBSD__
1830         s = splnet();
1831 #else
1832         s = splimp();
1833 #endif
1834         IFQ_LOCK(cl->cl_hif->hif_ifq);
1835
1836         if (rsc != NULL) {
1837                 if (rsc->m1 == 0 && rsc->m2 == 0) {
1838                         if (cl->cl_rsc != NULL) {
1839                                 if (!qempty(cl->cl_q))
1840                                         hfsc_purgeq(cl);
1841                                 free(cl->cl_rsc, M_DEVBUF);
1842                                 cl->cl_rsc = NULL;
1843                         }
1844                 } else {
1845                         if (cl->cl_rsc == NULL)
1846                                 cl->cl_rsc = rsc_tmp;
1847                         sc2isc(rsc, cl->cl_rsc);
1848                         rtsc_init(&cl->cl_deadline, cl->cl_rsc, cur_time,
1849                             cl->cl_cumul);
1850                         cl->cl_eligible = cl->cl_deadline;
1851                         if (cl->cl_rsc->sm1 <= cl->cl_rsc->sm2) {
1852                                 cl->cl_eligible.dx = 0;
1853                                 cl->cl_eligible.dy = 0;
1854                         }
1855                 }
1856         }
1857
1858         if (fsc != NULL) {
1859                 if (fsc->m1 == 0 && fsc->m2 == 0) {
1860                         if (cl->cl_fsc != NULL) {
1861                                 if (!qempty(cl->cl_q))
1862                                         hfsc_purgeq(cl);
1863                                 free(cl->cl_fsc, M_DEVBUF);
1864                                 cl->cl_fsc = NULL;
1865                         }
1866                 } else {
1867                         if (cl->cl_fsc == NULL)
1868                                 cl->cl_fsc = fsc_tmp;
1869                         sc2isc(fsc, cl->cl_fsc);
1870                         rtsc_init(&cl->cl_virtual, cl->cl_fsc, cl->cl_vt,
1871                             cl->cl_total);
1872                 }
1873         }
1874
1875         if (usc != NULL) {
1876                 if (usc->m1 == 0 && usc->m2 == 0) {
1877                         if (cl->cl_usc != NULL) {
1878                                 free(cl->cl_usc, M_DEVBUF);
1879                                 cl->cl_usc = NULL;
1880                                 cl->cl_myf = 0;
1881                         }
1882                 } else {
1883                         if (cl->cl_usc == NULL)
1884                                 cl->cl_usc = usc_tmp;
1885                         sc2isc(usc, cl->cl_usc);
1886                         rtsc_init(&cl->cl_ulimit, cl->cl_usc, cur_time,
1887                             cl->cl_total);
1888                 }
1889         }
1890
1891         if (!qempty(cl->cl_q)) {
1892                 if (cl->cl_rsc != NULL)
1893                         update_ed(cl, m_pktlen(qhead(cl->cl_q)));
1894                 if (cl->cl_fsc != NULL)
1895                         update_vf(cl, 0, cur_time);
1896                 /* is this enough? */
1897         }
1898
1899         IFQ_UNLOCK(cl->cl_hif->hif_ifq);
1900         splx(s);
1901
1902         return (0);
1903 }
1904
1905 /*
1906  * hfsc device interface
1907  */
1908 int
1909 hfscopen(dev, flag, fmt, p)
1910         dev_t dev;
1911         int flag, fmt;
1912 #if (__FreeBSD_version > 500000)
1913         struct thread *p;
1914 #else
1915         struct proc *p;
1916 #endif
1917 {
1918         if (machclk_freq == 0)
1919                 init_machclk();
1920
1921         if (machclk_freq == 0) {
1922                 printf("hfsc: no cpu clock available!\n");
1923                 return (ENXIO);
1924         }
1925
1926         /* everything will be done when the queueing scheme is attached. */
1927         return 0;
1928 }
1929
1930 int
1931 hfscclose(dev, flag, fmt, p)
1932         dev_t dev;
1933         int flag, fmt;
1934 #if (__FreeBSD_version > 500000)
1935         struct thread *p;
1936 #else
1937         struct proc *p;
1938 #endif
1939 {
1940         struct hfsc_if *hif;
1941         int err, error = 0;
1942
1943         while ((hif = hif_list) != NULL) {
1944                 /* destroy all */
1945                 if (ALTQ_IS_ENABLED(hif->hif_ifq))
1946                         altq_disable(hif->hif_ifq);
1947
1948                 err = altq_detach(hif->hif_ifq);
1949                 if (err == 0)
1950                         err = hfsc_detach(hif);
1951                 if (err != 0 && error == 0)
1952                         error = err;
1953         }
1954
1955         return error;
1956 }
1957
1958 int
1959 hfscioctl(dev, cmd, addr, flag, p)
1960         dev_t dev;
1961         ioctlcmd_t cmd;
1962         caddr_t addr;
1963         int flag;
1964 #if (__FreeBSD_version > 500000)
1965         struct thread *p;
1966 #else
1967         struct proc *p;
1968 #endif
1969 {
1970         struct hfsc_if *hif;
1971         struct hfsc_interface *ifacep;
1972         int     error = 0;
1973
1974         /* check super-user privilege */
1975         switch (cmd) {
1976         case HFSC_GETSTATS:
1977                 break;
1978         default:
1979 #if (__FreeBSD_version > 700000)
1980                 if ((error = priv_check(p, PRIV_ALTQ_MANAGE)) != 0)
1981                         return (error);
1982 #elsif (__FreeBSD_version > 400000)
1983                 if ((error = suser(p)) != 0)
1984                         return (error);
1985 #else
1986                 if ((error = suser(p->p_ucred, &p->p_acflag)) != 0)
1987                         return (error);
1988 #endif
1989                 break;
1990         }
1991
1992         switch (cmd) {
1993
1994         case HFSC_IF_ATTACH:
1995                 error = hfsccmd_if_attach((struct hfsc_attach *)addr);
1996                 break;
1997
1998         case HFSC_IF_DETACH:
1999                 error = hfsccmd_if_detach((struct hfsc_interface *)addr);
2000                 break;
2001
2002         case HFSC_ENABLE:
2003         case HFSC_DISABLE:
2004         case HFSC_CLEAR_HIERARCHY:
2005                 ifacep = (struct hfsc_interface *)addr;
2006                 if ((hif = altq_lookup(ifacep->hfsc_ifname,
2007                                        ALTQT_HFSC)) == NULL) {
2008                         error = EBADF;
2009                         break;
2010                 }
2011
2012                 switch (cmd) {
2013
2014                 case HFSC_ENABLE:
2015                         if (hif->hif_defaultclass == NULL) {
2016 #ifdef ALTQ_DEBUG
2017                                 printf("hfsc: no default class\n");
2018 #endif
2019                                 error = EINVAL;
2020                                 break;
2021                         }
2022                         error = altq_enable(hif->hif_ifq);
2023                         break;
2024
2025                 case HFSC_DISABLE:
2026                         error = altq_disable(hif->hif_ifq);
2027                         break;
2028
2029                 case HFSC_CLEAR_HIERARCHY:
2030                         hfsc_clear_interface(hif);
2031                         break;
2032                 }
2033                 break;
2034
2035         case HFSC_ADD_CLASS:
2036                 error = hfsccmd_add_class((struct hfsc_add_class *)addr);
2037                 break;
2038
2039         case HFSC_DEL_CLASS:
2040                 error = hfsccmd_delete_class((struct hfsc_delete_class *)addr);
2041                 break;
2042
2043         case HFSC_MOD_CLASS:
2044                 error = hfsccmd_modify_class((struct hfsc_modify_class *)addr);
2045                 break;
2046
2047         case HFSC_ADD_FILTER:
2048                 error = hfsccmd_add_filter((struct hfsc_add_filter *)addr);
2049                 break;
2050
2051         case HFSC_DEL_FILTER:
2052                 error = hfsccmd_delete_filter((struct hfsc_delete_filter *)addr);
2053                 break;
2054
2055         case HFSC_GETSTATS:
2056                 error = hfsccmd_class_stats((struct hfsc_class_stats *)addr);
2057                 break;
2058
2059         default:
2060                 error = EINVAL;
2061                 break;
2062         }
2063         return error;
2064 }
2065
2066 static int
2067 hfsccmd_if_attach(ap)
2068         struct hfsc_attach *ap;
2069 {
2070         struct hfsc_if *hif;
2071         struct ifnet *ifp;
2072         int error;
2073
2074         if ((ifp = ifunit(ap->iface.hfsc_ifname)) == NULL)
2075                 return (ENXIO);
2076
2077         if ((hif = hfsc_attach(&ifp->if_snd, ap->bandwidth)) == NULL)
2078                 return (ENOMEM);
2079
2080         /*
2081          * set HFSC to this ifnet structure.
2082          */
2083         if ((error = altq_attach(&ifp->if_snd, ALTQT_HFSC, hif,
2084                                  hfsc_enqueue, hfsc_dequeue, hfsc_request,
2085                                  &hif->hif_classifier, acc_classify)) != 0)
2086                 (void)hfsc_detach(hif);
2087
2088         return (error);
2089 }
2090
2091 static int
2092 hfsccmd_if_detach(ap)
2093         struct hfsc_interface *ap;
2094 {
2095         struct hfsc_if *hif;
2096         int error;
2097
2098         if ((hif = altq_lookup(ap->hfsc_ifname, ALTQT_HFSC)) == NULL)
2099                 return (EBADF);
2100
2101         if (ALTQ_IS_ENABLED(hif->hif_ifq))
2102                 altq_disable(hif->hif_ifq);
2103
2104         if ((error = altq_detach(hif->hif_ifq)))
2105                 return (error);
2106
2107         return hfsc_detach(hif);
2108 }
2109
2110 static int
2111 hfsccmd_add_class(ap)
2112         struct hfsc_add_class *ap;
2113 {
2114         struct hfsc_if *hif;
2115         struct hfsc_class *cl, *parent;
2116         int     i;
2117
2118         if ((hif = altq_lookup(ap->iface.hfsc_ifname, ALTQT_HFSC)) == NULL)
2119                 return (EBADF);
2120
2121         if (ap->parent_handle == HFSC_NULLCLASS_HANDLE &&
2122             hif->hif_rootclass == NULL)
2123                 parent = NULL;
2124         else if ((parent = clh_to_clp(hif, ap->parent_handle)) == NULL)
2125                 return (EINVAL);
2126
2127         /* assign a class handle (use a free slot number for now) */
2128         for (i = 1; i < HFSC_MAX_CLASSES; i++)
2129                 if (hif->hif_class_tbl[i] == NULL)
2130                         break;
2131         if (i == HFSC_MAX_CLASSES)
2132                 return (EBUSY);
2133
2134         if ((cl = hfsc_class_create(hif, &ap->service_curve, NULL, NULL,
2135             parent, ap->qlimit, ap->flags, i)) == NULL)
2136                 return (ENOMEM);
2137
2138         /* return a class handle to the user */
2139         ap->class_handle = i;
2140
2141         return (0);
2142 }
2143
2144 static int
2145 hfsccmd_delete_class(ap)
2146         struct hfsc_delete_class *ap;
2147 {
2148         struct hfsc_if *hif;
2149         struct hfsc_class *cl;
2150
2151         if ((hif = altq_lookup(ap->iface.hfsc_ifname, ALTQT_HFSC)) == NULL)
2152                 return (EBADF);
2153
2154         if ((cl = clh_to_clp(hif, ap->class_handle)) == NULL)
2155                 return (EINVAL);
2156
2157         return hfsc_class_destroy(cl);
2158 }
2159
2160 static int
2161 hfsccmd_modify_class(ap)
2162         struct hfsc_modify_class *ap;
2163 {
2164         struct hfsc_if *hif;
2165         struct hfsc_class *cl;
2166         struct service_curve *rsc = NULL;
2167         struct service_curve *fsc = NULL;
2168         struct service_curve *usc = NULL;
2169
2170         if ((hif = altq_lookup(ap->iface.hfsc_ifname, ALTQT_HFSC)) == NULL)
2171                 return (EBADF);
2172
2173         if ((cl = clh_to_clp(hif, ap->class_handle)) == NULL)
2174                 return (EINVAL);
2175
2176         if (ap->sctype & HFSC_REALTIMESC)
2177                 rsc = &ap->service_curve;
2178         if (ap->sctype & HFSC_LINKSHARINGSC)
2179                 fsc = &ap->service_curve;
2180         if (ap->sctype & HFSC_UPPERLIMITSC)
2181                 usc = &ap->service_curve;
2182
2183         return hfsc_class_modify(cl, rsc, fsc, usc);
2184 }
2185
2186 static int
2187 hfsccmd_add_filter(ap)
2188         struct hfsc_add_filter *ap;
2189 {
2190         struct hfsc_if *hif;
2191         struct hfsc_class *cl;
2192
2193         if ((hif = altq_lookup(ap->iface.hfsc_ifname, ALTQT_HFSC)) == NULL)
2194                 return (EBADF);
2195
2196         if ((cl = clh_to_clp(hif, ap->class_handle)) == NULL)
2197                 return (EINVAL);
2198
2199         if (is_a_parent_class(cl)) {
2200 #ifdef ALTQ_DEBUG
2201                 printf("hfsccmd_add_filter: not a leaf class!\n");
2202 #endif
2203                 return (EINVAL);
2204         }
2205
2206         return acc_add_filter(&hif->hif_classifier, &ap->filter,
2207                               cl, &ap->filter_handle);
2208 }
2209
2210 static int
2211 hfsccmd_delete_filter(ap)
2212         struct hfsc_delete_filter *ap;
2213 {
2214         struct hfsc_if *hif;
2215
2216         if ((hif = altq_lookup(ap->iface.hfsc_ifname, ALTQT_HFSC)) == NULL)
2217                 return (EBADF);
2218
2219         return acc_delete_filter(&hif->hif_classifier,
2220                                  ap->filter_handle);
2221 }
2222
2223 static int
2224 hfsccmd_class_stats(ap)
2225         struct hfsc_class_stats *ap;
2226 {
2227         struct hfsc_if *hif;
2228         struct hfsc_class *cl;
2229         struct hfsc_classstats stats, *usp;
2230         int     n, nclasses, error;
2231
2232         if ((hif = altq_lookup(ap->iface.hfsc_ifname, ALTQT_HFSC)) == NULL)
2233                 return (EBADF);
2234
2235         ap->cur_time = read_machclk();
2236         ap->machclk_freq = machclk_freq;
2237         ap->hif_classes = hif->hif_classes;
2238         ap->hif_packets = hif->hif_packets;
2239
2240         /* skip the first N classes in the tree */
2241         nclasses = ap->nskip;
2242         for (cl = hif->hif_rootclass, n = 0; cl != NULL && n < nclasses;
2243              cl = hfsc_nextclass(cl), n++)
2244                 ;
2245         if (n != nclasses)
2246                 return (EINVAL);
2247
2248         /* then, read the next N classes in the tree */
2249         nclasses = ap->nclasses;
2250         usp = ap->stats;
2251         for (n = 0; cl != NULL && n < nclasses; cl = hfsc_nextclass(cl), n++) {
2252
2253                 get_class_stats(&stats, cl);
2254
2255                 if ((error = copyout((caddr_t)&stats, (caddr_t)usp++,
2256                                      sizeof(stats))) != 0)
2257                         return (error);
2258         }
2259
2260         ap->nclasses = n;
2261
2262         return (0);
2263 }
2264
2265 #ifdef KLD_MODULE
2266
2267 static struct altqsw hfsc_sw =
2268         {"hfsc", hfscopen, hfscclose, hfscioctl};
2269
2270 ALTQ_MODULE(altq_hfsc, ALTQT_HFSC, &hfsc_sw);
2271 MODULE_DEPEND(altq_hfsc, altq_red, 1, 1, 1);
2272 MODULE_DEPEND(altq_hfsc, altq_rio, 1, 1, 1);
2273
2274 #endif /* KLD_MODULE */
2275 #endif /* ALTQ3_COMPAT */
2276
2277 #endif /* ALTQ_HFSC */