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