bsd/net/pktsched/pktsched_tcq.c

   1 /*
   2  * Copyright (c) 2011-2012 Apple Inc. All rights reserved.
   3  *
   4  * @APPLE_OSREFERENCE_LICENSE_HEADER_START@
   5  *
   6  * This file contains Original Code and/or Modifications of Original Code
   7  * as defined in and that are subject to the Apple Public Source License
   8  * Version 2.0 (the 'License'). You may not use this file except in
   9  * compliance with the License. The rights granted to you under the License
  10  * may not be used to create, or enable the creation or redistribution of,
  11  * unlawful or unlicensed copies of an Apple operating system, or to
  12  * circumvent, violate, or enable the circumvention or violation of, any
  13  * terms of an Apple operating system software license agreement.
  14  *
  15  * Please obtain a copy of the License at
  16  * http://www.opensource.apple.com/apsl/ and read it before using this file.
  17  *
  18  * The Original Code and all software distributed under the License are
  19  * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
  20  * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
  21  * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
  22  * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
  23  * Please see the License for the specific language governing rights and
  24  * limitations under the License.
  25  *
  26  * @APPLE_OSREFERENCE_LICENSE_HEADER_END@
  27  */
  28
  29 /*
  30  * traffic class queue
  31  */
  32
  33 #include <sys/cdefs.h>
  34 #include <sys/param.h>
  35 #include <sys/malloc.h>
  36 #include <sys/mbuf.h>
  37 #include <sys/systm.h>
  38 #include <sys/errno.h>
  39 #include <sys/kernel.h>
  40 #include <sys/syslog.h>
  41
  42 #include <kern/zalloc.h>
  43
  44 #include <net/if.h>
  45 #include <net/net_osdep.h>
  46
  47 #include <net/pktsched/pktsched_tcq.h>
  48 #include <netinet/in.h>
  49
  50 /*
  51  * function prototypes
  52  */
  53 static int tcq_enqueue_ifclassq(struct ifclassq *, struct mbuf *);
  54 static struct mbuf *tcq_dequeue_tc_ifclassq(struct ifclassq *,
  55     mbuf_svc_class_t, cqdq_op_t);
  56 static int tcq_request_ifclassq(struct ifclassq *, cqrq_t, void *);
  57 static int tcq_clear_interface(struct tcq_if *);
  58 static struct tcq_class *tcq_class_create(struct tcq_if *, int, u_int32_t,
  59     int, u_int32_t);
  60 static int tcq_class_destroy(struct tcq_if *, struct tcq_class *);
  61 static int tcq_destroy_locked(struct tcq_if *);
  62 static inline int tcq_addq(struct tcq_class *, struct mbuf *,
  63     struct pf_mtag *);
  64 static inline struct mbuf *tcq_getq(struct tcq_class *);
  65 static inline struct mbuf *tcq_pollq(struct tcq_class *);
  66 static void tcq_purgeq(struct tcq_if *, struct tcq_class *, u_int32_t,
  67     u_int32_t *, u_int32_t *);
  68 static void tcq_purge_sc(struct tcq_if *, cqrq_purge_sc_t *);
  69 static void tcq_updateq(struct tcq_if *, struct tcq_class *, cqev_t);
  70 static int tcq_throttle(struct tcq_if *, cqrq_throttle_t *);
  71 static int tcq_resumeq(struct tcq_if *, struct tcq_class *);
  72 static int tcq_suspendq(struct tcq_if *, struct tcq_class *);
  73 static int tcq_stat_sc(struct tcq_if *, cqrq_stat_sc_t *);
  74 static struct mbuf *tcq_dequeue_cl(struct tcq_if *, struct tcq_class *,
  75     mbuf_svc_class_t, cqdq_op_t);
  76 static inline struct tcq_class *tcq_clh_to_clp(struct tcq_if *, u_int32_t);
  77 static const char *tcq_style(struct tcq_if *);
  78
  79 #define TCQ_ZONE_MAX    32              /* maximum elements in zone */
  80 #define TCQ_ZONE_NAME   "pktsched_tcq"  /* zone name */
  81
  82 static unsigned int tcq_size;           /* size of zone element */
  83 static struct zone *tcq_zone;           /* zone for tcq */
  84
  85 #define TCQ_CL_ZONE_MAX 32              /* maximum elements in zone */
  86 #define TCQ_CL_ZONE_NAME "pktsched_tcq_cl" /* zone name */
  87
  88 static unsigned int tcq_cl_size;        /* size of zone element */
  89 static struct zone *tcq_cl_zone;        /* zone for tcq_class */
  90
  91 void
  92 tcq_init(void)
  93 {
  94         tcq_size = sizeof (struct tcq_if);
  95         tcq_zone = zinit(tcq_size, TCQ_ZONE_MAX * tcq_size,
  96             0, TCQ_ZONE_NAME);
  97         if (tcq_zone == NULL) {
  98                 panic("%s: failed allocating %s", __func__, TCQ_ZONE_NAME);
  99                 /* NOTREACHED */
 100         }
 101         zone_change(tcq_zone, Z_EXPAND, TRUE);
 102         zone_change(tcq_zone, Z_CALLERACCT, TRUE);
 103
 104         tcq_cl_size = sizeof (struct tcq_class);
 105         tcq_cl_zone = zinit(tcq_cl_size, TCQ_CL_ZONE_MAX * tcq_cl_size,
 106             0, TCQ_CL_ZONE_NAME);
 107         if (tcq_cl_zone == NULL) {
 108                 panic("%s: failed allocating %s", __func__, TCQ_CL_ZONE_NAME);
 109                 /* NOTREACHED */
 110         }
 111         zone_change(tcq_cl_zone, Z_EXPAND, TRUE);
 112         zone_change(tcq_cl_zone, Z_CALLERACCT, TRUE);
 113 }
 114
 115 struct tcq_if *
 116 tcq_alloc(struct ifnet *ifp, int how, boolean_t altq)
 117 {
 118         struct tcq_if   *tif;
 119
 120         tif = (how == M_WAITOK) ? zalloc(tcq_zone) : zalloc_noblock(tcq_zone);
 121         if (tif == NULL)
 122                 return (NULL);
 123
 124         bzero(tif, tcq_size);
 125         tif->tif_maxpri = -1;
 126         tif->tif_ifq = &ifp->if_snd;
 127         if (altq)
 128                 tif->tif_flags |= TCQIFF_ALTQ;
 129
 130         if (pktsched_verbose) {
 131                 log(LOG_DEBUG, "%s: %s scheduler allocated\n",
 132                     if_name(ifp), tcq_style(tif));
 133         }
 134
 135         return (tif);
 136 }
 137
 138 int
 139 tcq_destroy(struct tcq_if *tif)
 140 {
 141         struct ifclassq *ifq = tif->tif_ifq;
 142         int err;
 143
 144         IFCQ_LOCK(ifq);
 145         err = tcq_destroy_locked(tif);
 146         IFCQ_UNLOCK(ifq);
 147
 148         return (err);
 149 }
 150
 151 static int
 152 tcq_destroy_locked(struct tcq_if *tif)
 153 {
 154         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 155
 156         (void) tcq_clear_interface(tif);
 157
 158         if (pktsched_verbose) {
 159                 log(LOG_DEBUG, "%s: %s scheduler destroyed\n",
 160                     if_name(TCQIF_IFP(tif)), tcq_style(tif));
 161         }
 162
 163         zfree(tcq_zone, tif);
 164
 165         return (0);
 166 }
 167
 168 /*
 169  * bring the interface back to the initial state by discarding
 170  * all the filters and classes.
 171  */
 172 static int
 173 tcq_clear_interface(struct tcq_if *tif)
 174 {
 175         struct tcq_class        *cl;
 176         int pri;
 177
 178         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 179
 180         /* clear out the classes */
 181         for (pri = 0; pri <= tif->tif_maxpri; pri++)
 182                 if ((cl = tif->tif_classes[pri]) != NULL)
 183                         tcq_class_destroy(tif, cl);
 184
 185         return (0);
 186 }
 187
 188 /* discard all the queued packets on the interface */
 189 void
 190 tcq_purge(struct tcq_if *tif)
 191 {
 192         struct tcq_class *cl;
 193         int pri;
 194
 195         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 196
 197         for (pri = 0; pri <= tif->tif_maxpri; pri++) {
 198                 if ((cl = tif->tif_classes[pri]) != NULL && !qempty(&cl->cl_q))
 199                         tcq_purgeq(tif, cl, 0, NULL, NULL);
 200         }
 201 #if !PF_ALTQ
 202         /*
 203          * This assertion is safe to be made only when PF_ALTQ is not
 204          * configured; otherwise, IFCQ_LEN represents the sum of the
 205          * packets managed by ifcq_disc and altq_disc instances, which
 206          * is possible when transitioning between the two.
 207          */
 208         VERIFY(IFCQ_LEN(tif->tif_ifq) == 0);
 209 #endif /* !PF_ALTQ */
 210 }
 211
 212 static void
 213 tcq_purge_sc(struct tcq_if *tif, cqrq_purge_sc_t *pr)
 214 {
 215         struct ifclassq *ifq = tif->tif_ifq;
 216         u_int32_t i;
 217
 218         IFCQ_LOCK_ASSERT_HELD(ifq);
 219
 220         VERIFY(pr->sc == MBUF_SC_UNSPEC || MBUF_VALID_SC(pr->sc));
 221         VERIFY(pr->flow != 0);
 222
 223         if (pr->sc != MBUF_SC_UNSPEC) {
 224                 i = MBUF_SCIDX(pr->sc);
 225                 VERIFY(i < IFCQ_SC_MAX);
 226
 227                 tcq_purgeq(tif, ifq->ifcq_disc_slots[i].cl,
 228                     pr->flow, &pr->packets, &pr->bytes);
 229         } else {
 230                 u_int32_t cnt, len;
 231
 232                 pr->packets = 0;
 233                 pr->bytes = 0;
 234
 235                 for (i = 0; i < IFCQ_SC_MAX; i++) {
 236                         tcq_purgeq(tif, ifq->ifcq_disc_slots[i].cl,
 237                             pr->flow, &cnt, &len);
 238                         pr->packets += cnt;
 239                         pr->bytes += len;
 240                 }
 241         }
 242 }
 243
 244 void
 245 tcq_event(struct tcq_if *tif, cqev_t ev)
 246 {
 247         struct tcq_class *cl;
 248         int pri;
 249
 250         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 251
 252         for (pri = 0; pri <= tif->tif_maxpri; pri++)
 253                 if ((cl = tif->tif_classes[pri]) != NULL)
 254                         tcq_updateq(tif, cl, ev);
 255 }
 256
 257 int
 258 tcq_add_queue(struct tcq_if *tif, int priority, u_int32_t qlimit,
 259     int flags, u_int32_t qid, struct tcq_class **clp)
 260 {
 261         struct tcq_class *cl;
 262
 263         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 264
 265         /* check parameters */
 266         if (priority >= TCQ_MAXPRI)
 267                 return (EINVAL);
 268         if (tif->tif_classes[priority] != NULL)
 269                 return (EBUSY);
 270         if (tcq_clh_to_clp(tif, qid) != NULL)
 271                 return (EBUSY);
 272
 273         cl = tcq_class_create(tif, priority, qlimit, flags, qid);
 274         if (cl == NULL)
 275                 return (ENOMEM);
 276
 277         if (clp != NULL)
 278                 *clp = cl;
 279
 280         return (0);
 281 }
 282
 283 static struct tcq_class *
 284 tcq_class_create(struct tcq_if *tif, int pri, u_int32_t qlimit,
 285     int flags, u_int32_t qid)
 286 {
 287         struct ifnet *ifp;
 288         struct ifclassq *ifq;
 289         struct tcq_class *cl;
 290
 291         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 292
 293         /* Sanitize flags unless internally configured */
 294         if (tif->tif_flags & TCQIFF_ALTQ)
 295                 flags &= TQCF_USERFLAGS;
 296
 297 #if !CLASSQ_RED
 298         if (flags & TQCF_RED) {
 299                 log(LOG_ERR, "%s: %s RED not available!\n",
 300                     if_name(TCQIF_IFP(tif)), tcq_style(tif));
 301                 return (NULL);
 302         }
 303 #endif /* !CLASSQ_RED */
 304
 305 #if !CLASSQ_RIO
 306         if (flags & TQCF_RIO) {
 307                 log(LOG_ERR, "%s: %s RIO not available!\n",
 308                     if_name(TCQIF_IFP(tif)), tcq_style(tif));
 309                 return (NULL);
 310         }
 311 #endif /* CLASSQ_RIO */
 312
 313 #if !CLASSQ_BLUE
 314         if (flags & TQCF_BLUE) {
 315                 log(LOG_ERR, "%s: %s BLUE not available!\n",
 316                     if_name(TCQIF_IFP(tif)), tcq_style(tif));
 317                 return (NULL);
 318         }
 319 #endif /* CLASSQ_BLUE */
 320
 321         /* These are mutually exclusive */
 322         if ((flags & (TQCF_RED|TQCF_RIO|TQCF_BLUE|TQCF_SFB)) &&
 323             (flags & (TQCF_RED|TQCF_RIO|TQCF_BLUE|TQCF_SFB)) != TQCF_RED &&
 324             (flags & (TQCF_RED|TQCF_RIO|TQCF_BLUE|TQCF_SFB)) != TQCF_RIO &&
 325             (flags & (TQCF_RED|TQCF_RIO|TQCF_BLUE|TQCF_SFB)) != TQCF_BLUE &&
 326             (flags & (TQCF_RED|TQCF_RIO|TQCF_BLUE|TQCF_SFB)) != TQCF_SFB) {
 327                 log(LOG_ERR, "%s: %s more than one RED|RIO|BLUE|SFB\n",
 328                     if_name(TCQIF_IFP(tif)), tcq_style(tif));
 329                 return (NULL);
 330         }
 331
 332         ifq = tif->tif_ifq;
 333         ifp = TCQIF_IFP(tif);
 334
 335         if ((cl = tif->tif_classes[pri]) != NULL) {
 336                 /* modify the class instead of creating a new one */
 337                 if (!qempty(&cl->cl_q))
 338                         tcq_purgeq(tif, cl, 0, NULL, NULL);
 339 #if CLASSQ_RIO
 340                 if (q_is_rio(&cl->cl_q))
 341                         rio_destroy(cl->cl_rio);
 342 #endif /* CLASSQ_RIO */
 343 #if CLASSQ_RED
 344                 if (q_is_red(&cl->cl_q))
 345                         red_destroy(cl->cl_red);
 346 #endif /* CLASSQ_RED */
 347 #if CLASSQ_BLUE
 348                 if (q_is_blue(&cl->cl_q))
 349                         blue_destroy(cl->cl_blue);
 350 #endif /* CLASSQ_BLUE */
 351                 if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL)
 352                         sfb_destroy(cl->cl_sfb);
 353                 cl->cl_qalg.ptr = NULL;
 354                 qtype(&cl->cl_q) = Q_DROPTAIL;
 355                 qstate(&cl->cl_q) = QS_RUNNING;
 356         } else {
 357                 cl = zalloc(tcq_cl_zone);
 358                 if (cl == NULL)
 359                         return (NULL);
 360
 361                 bzero(cl, tcq_cl_size);
 362         }
 363
 364         tif->tif_classes[pri] = cl;
 365         if (flags & TQCF_DEFAULTCLASS)
 366                 tif->tif_default = cl;
 367         if (qlimit == 0 || qlimit > IFCQ_MAXLEN(ifq)) {
 368                 qlimit = IFCQ_MAXLEN(ifq);
 369                 if (qlimit == 0)
 370                         qlimit = DEFAULT_QLIMIT;  /* use default */
 371         }
 372         _qinit(&cl->cl_q, Q_DROPTAIL, qlimit);
 373         cl->cl_flags = flags;
 374         cl->cl_pri = pri;
 375         if (pri > tif->tif_maxpri)
 376                 tif->tif_maxpri = pri;
 377         cl->cl_tif = tif;
 378         cl->cl_handle = qid;
 379
 380         if (flags & (TQCF_RED|TQCF_RIO|TQCF_BLUE|TQCF_SFB)) {
 381 #if CLASSQ_RED || CLASSQ_RIO
 382                 u_int64_t ifbandwidth = ifnet_output_linkrate(ifp);
 383                 int pkttime;
 384 #endif /* CLASSQ_RED || CLASSQ_RIO */
 385
 386                 cl->cl_qflags = 0;
 387                 if (flags & TQCF_ECN) {
 388                         if (flags & TQCF_BLUE)
 389                                 cl->cl_qflags |= BLUEF_ECN;
 390                         else if (flags & TQCF_SFB)
 391                                 cl->cl_qflags |= SFBF_ECN;
 392                         else if (flags & TQCF_RED)
 393                                 cl->cl_qflags |= REDF_ECN;
 394                         else if (flags & TQCF_RIO)
 395                                 cl->cl_qflags |= RIOF_ECN;
 396                 }
 397                 if (flags & TQCF_FLOWCTL) {
 398                         if (flags & TQCF_SFB)
 399                                 cl->cl_qflags |= SFBF_FLOWCTL;
 400                 }
 401                 if (flags & TQCF_CLEARDSCP) {
 402                         if (flags & TQCF_RIO)
 403                                 cl->cl_qflags |= RIOF_CLEARDSCP;
 404                 }
 405 #if CLASSQ_RED || CLASSQ_RIO
 406                 /*
 407                  * XXX: RED & RIO should be watching link speed and MTU
 408                  *      events and recompute pkttime accordingly.
 409                  */
 410                 if (ifbandwidth < 8)
 411                         pkttime = 1000 * 1000 * 1000; /* 1 sec */
 412                 else
 413                         pkttime = (int64_t)ifp->if_mtu * 1000 * 1000 * 1000 /
 414                             (ifbandwidth / 8);
 415
 416                 /* Test for exclusivity {RED,RIO,BLUE,SFB} was done above */
 417 #if CLASSQ_RED
 418                 if (flags & TQCF_RED) {
 419                         cl->cl_red = red_alloc(ifp, 0, 0,
 420                             qlimit(&cl->cl_q) * 10/100,
 421                             qlimit(&cl->cl_q) * 30/100,
 422                             cl->cl_qflags, pkttime);
 423                         if (cl->cl_red != NULL)
 424                                 qtype(&cl->cl_q) = Q_RED;
 425                 }
 426 #endif /* CLASSQ_RED */
 427 #if CLASSQ_RIO
 428                 if (flags & TQCF_RIO) {
 429                         cl->cl_rio =
 430                             rio_alloc(ifp, 0, NULL, cl->cl_qflags, pkttime);
 431                         if (cl->cl_rio != NULL)
 432                                 qtype(&cl->cl_q) = Q_RIO;
 433                 }
 434 #endif /* CLASSQ_RIO */
 435 #endif /* CLASSQ_RED || CLASSQ_RIO */
 436 #if CLASSQ_BLUE
 437                 if (flags & TQCF_BLUE) {
 438                         cl->cl_blue = blue_alloc(ifp, 0, 0, cl->cl_qflags);
 439                         if (cl->cl_blue != NULL)
 440                                 qtype(&cl->cl_q) = Q_BLUE;
 441                 }
 442 #endif /* CLASSQ_BLUE */
 443                 if (flags & TQCF_SFB) {
 444                         if (!(cl->cl_flags & TQCF_LAZY))
 445                                 cl->cl_sfb = sfb_alloc(ifp, cl->cl_handle,
 446                                     qlimit(&cl->cl_q), cl->cl_qflags);
 447                         if (cl->cl_sfb != NULL || (cl->cl_flags & TQCF_LAZY))
 448                                 qtype(&cl->cl_q) = Q_SFB;
 449                 }
 450         }
 451
 452         if (pktsched_verbose) {
 453                 log(LOG_DEBUG, "%s: %s created qid=%d pri=%d qlimit=%d "
 454                     "flags=%b\n", if_name(ifp), tcq_style(tif),
 455                     cl->cl_handle, cl->cl_pri, qlimit, flags, TQCF_BITS);
 456         }
 457
 458         return (cl);
 459 }
 460
 461 int
 462 tcq_remove_queue(struct tcq_if *tif, u_int32_t qid)
 463 {
 464         struct tcq_class *cl;
 465
 466         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 467
 468         if ((cl = tcq_clh_to_clp(tif, qid)) == NULL)
 469                 return (EINVAL);
 470
 471         return (tcq_class_destroy(tif, cl));
 472 }
 473
 474 static int
 475 tcq_class_destroy(struct tcq_if *tif, struct tcq_class *cl)
 476 {
 477         struct ifclassq *ifq = tif->tif_ifq;
 478         int pri;
 479
 480         IFCQ_LOCK_ASSERT_HELD(ifq);
 481
 482         if (!qempty(&cl->cl_q))
 483                 tcq_purgeq(tif, cl, 0, NULL, NULL);
 484
 485         tif->tif_classes[cl->cl_pri] = NULL;
 486         if (tif->tif_maxpri == cl->cl_pri) {
 487                 for (pri = cl->cl_pri; pri >= 0; pri--)
 488                         if (tif->tif_classes[pri] != NULL) {
 489                                 tif->tif_maxpri = pri;
 490                                 break;
 491                         }
 492                 if (pri < 0)
 493                         tif->tif_maxpri = -1;
 494         }
 495
 496         if (tif->tif_default == cl)
 497                 tif->tif_default = NULL;
 498
 499         if (cl->cl_qalg.ptr != NULL) {
 500 #if CLASSQ_RIO
 501                 if (q_is_rio(&cl->cl_q))
 502                         rio_destroy(cl->cl_rio);
 503 #endif /* CLASSQ_RIO */
 504 #if CLASSQ_RED
 505                 if (q_is_red(&cl->cl_q))
 506                         red_destroy(cl->cl_red);
 507 #endif /* CLASSQ_RED */
 508 #if CLASSQ_BLUE
 509                 if (q_is_blue(&cl->cl_q))
 510                         blue_destroy(cl->cl_blue);
 511 #endif /* CLASSQ_BLUE */
 512                 if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL)
 513                         sfb_destroy(cl->cl_sfb);
 514                 cl->cl_qalg.ptr = NULL;
 515                 qtype(&cl->cl_q) = Q_DROPTAIL;
 516                 qstate(&cl->cl_q) = QS_RUNNING;
 517         }
 518
 519         if (pktsched_verbose) {
 520                 log(LOG_DEBUG, "%s: %s destroyed qid=%d pri=%d\n",
 521                     if_name(TCQIF_IFP(tif)), tcq_style(tif),
 522                     cl->cl_handle, cl->cl_pri);
 523         }
 524
 525         zfree(tcq_cl_zone, cl);
 526         return (0);
 527 }
 528
 529 int
 530 tcq_enqueue(struct tcq_if *tif, struct tcq_class *cl, struct mbuf *m,
 531     struct pf_mtag *t)
 532 {
 533         struct ifclassq *ifq = tif->tif_ifq;
 534         int len, ret;
 535
 536         IFCQ_LOCK_ASSERT_HELD(ifq);
 537         VERIFY(cl == NULL || cl->cl_tif == tif);
 538
 539         if (cl == NULL) {
 540 #if PF_ALTQ
 541                 cl = tcq_clh_to_clp(tif, t->pftag_qid);
 542 #else /* !PF_ALTQ */
 543                 cl = tcq_clh_to_clp(tif, 0);
 544 #endif /* !PF_ALTQ */
 545                 if (cl == NULL) {
 546                         cl = tif->tif_default;
 547                         if (cl == NULL) {
 548                                 IFCQ_CONVERT_LOCK(ifq);
 549                                 m_freem(m);
 550                                 return (ENOBUFS);
 551                         }
 552                 }
 553         }
 554
 555         len = m_pktlen(m);
 556
 557         ret = tcq_addq(cl, m, t);
 558         if (ret != 0) {
 559                 if (ret == CLASSQEQ_SUCCESS_FC) {
 560                         /* packet enqueued, return advisory feedback */
 561                         ret = EQFULL;
 562                 } else {
 563                         VERIFY(ret == CLASSQEQ_DROPPED ||
 564                             ret == CLASSQEQ_DROPPED_FC ||
 565                             ret == CLASSQEQ_DROPPED_SP);
 566                         /* packet has been freed in tcq_addq */
 567                         PKTCNTR_ADD(&cl->cl_dropcnt, 1, len);
 568                         IFCQ_DROP_ADD(ifq, 1, len);
 569                         switch (ret) {
 570                         case CLASSQEQ_DROPPED:
 571                                 return (ENOBUFS);
 572                         case CLASSQEQ_DROPPED_FC:
 573                                 return (EQFULL);
 574                         case CLASSQEQ_DROPPED_SP:
 575                                 return (EQSUSPENDED);
 576                         }
 577                         /* NOT REACHED */
 578                 }
 579         }
 580         IFCQ_INC_LEN(ifq);
 581
 582         /* successfully queued. */
 583         return (ret);
 584 }
 585
 586 /*
 587  * note: CLASSQDQ_POLL returns the next packet without removing the packet
 588  *      from the queue.  CLASSQDQ_REMOVE is a normal dequeue operation.
 589  *      CLASSQDQ_REMOVE must return the same packet if called immediately
 590  *      after CLASSQDQ_POLL.
 591  */
 592 struct mbuf *
 593 tcq_dequeue_tc(struct tcq_if *tif, mbuf_svc_class_t sc, cqdq_op_t op)
 594 {
 595         return (tcq_dequeue_cl(tif, NULL, sc, op));
 596 }
 597
 598 static struct mbuf *
 599 tcq_dequeue_cl(struct tcq_if *tif, struct tcq_class *cl,
 600     mbuf_svc_class_t sc, cqdq_op_t op)
 601 {
 602         struct ifclassq *ifq = tif->tif_ifq;
 603         struct mbuf *m;
 604
 605         IFCQ_LOCK_ASSERT_HELD(ifq);
 606
 607         if (cl == NULL) {
 608                 cl = tcq_clh_to_clp(tif, MBUF_SCIDX(sc));
 609                 if (cl == NULL)
 610                         return (NULL);
 611         }
 612
 613         if (qempty(&cl->cl_q))
 614                 return (NULL);
 615
 616         VERIFY(!IFCQ_IS_EMPTY(ifq));
 617
 618         if (op == CLASSQDQ_POLL)
 619                 return (tcq_pollq(cl));
 620
 621         m = tcq_getq(cl);
 622         if (m != NULL) {
 623                 IFCQ_DEC_LEN(ifq);
 624                 if (qempty(&cl->cl_q))
 625                         cl->cl_period++;
 626                 PKTCNTR_ADD(&cl->cl_xmitcnt, 1, m_pktlen(m));
 627                 IFCQ_XMIT_ADD(ifq, 1, m_pktlen(m));
 628         }
 629         return (m);
 630 }
 631
 632 static inline int
 633 tcq_addq(struct tcq_class *cl, struct mbuf *m, struct pf_mtag *t)
 634 {
 635         struct tcq_if *tif = cl->cl_tif;
 636         struct ifclassq *ifq = tif->tif_ifq;
 637
 638         IFCQ_LOCK_ASSERT_HELD(ifq);
 639
 640 #if CLASSQ_RIO
 641         if (q_is_rio(&cl->cl_q))
 642                 return (rio_addq(cl->cl_rio, &cl->cl_q, m, t));
 643         else
 644 #endif /* CLASSQ_RIO */
 645 #if CLASSQ_RED
 646         if (q_is_red(&cl->cl_q))
 647                 return (red_addq(cl->cl_red, &cl->cl_q, m, t));
 648         else
 649 #endif /* CLASSQ_RED */
 650 #if CLASSQ_BLUE
 651         if (q_is_blue(&cl->cl_q))
 652                 return (blue_addq(cl->cl_blue, &cl->cl_q, m, t));
 653         else
 654 #endif /* CLASSQ_BLUE */
 655         if (q_is_sfb(&cl->cl_q)) {
 656                 if (cl->cl_sfb == NULL) {
 657                         struct ifnet *ifp = TCQIF_IFP(tif);
 658
 659                         VERIFY(cl->cl_flags & TQCF_LAZY);
 660                         cl->cl_flags &= ~TQCF_LAZY;
 661                         IFCQ_CONVERT_LOCK(ifq);
 662
 663                         cl->cl_sfb = sfb_alloc(ifp, cl->cl_handle,
 664                             qlimit(&cl->cl_q), cl->cl_qflags);
 665                         if (cl->cl_sfb == NULL) {
 666                                 /* fall back to droptail */
 667                                 qtype(&cl->cl_q) = Q_DROPTAIL;
 668                                 cl->cl_flags &= ~TQCF_SFB;
 669                                 cl->cl_qflags &= ~(SFBF_ECN | SFBF_FLOWCTL);
 670
 671                                 log(LOG_ERR, "%s: %s SFB lazy allocation "
 672                                     "failed for qid=%d pri=%d, falling back "
 673                                     "to DROPTAIL\n", if_name(ifp),
 674                                     tcq_style(tif), cl->cl_handle,
 675                                     cl->cl_pri);
 676                         } else if (tif->tif_throttle != IFNET_THROTTLE_OFF) {
 677                                 /* if there's pending throttling, set it */
 678                                 cqrq_throttle_t tr = { 1, tif->tif_throttle };
 679                                 int err = tcq_throttle(tif, &tr);
 680
 681                                 if (err == EALREADY)
 682                                         err = 0;
 683                                 if (err != 0) {
 684                                         tr.level = IFNET_THROTTLE_OFF;
 685                                         (void) tcq_throttle(tif, &tr);
 686                                 }
 687                         }
 688                 }
 689                 if (cl->cl_sfb != NULL)
 690                         return (sfb_addq(cl->cl_sfb, &cl->cl_q, m, t));
 691         } else if (qlen(&cl->cl_q) >= qlimit(&cl->cl_q)) {
 692                 IFCQ_CONVERT_LOCK(ifq);
 693                 m_freem(m);
 694                 return (CLASSQEQ_DROPPED);
 695         }
 696
 697 #if PF_ECN
 698         if (cl->cl_flags & TQCF_CLEARDSCP)
 699                 write_dsfield(m, t, 0);
 700 #endif /* PF_ECN */
 701
 702         _addq(&cl->cl_q, m);
 703
 704         return (0);
 705 }
 706
 707 static inline struct mbuf *
 708 tcq_getq(struct tcq_class *cl)
 709 {
 710         IFCQ_LOCK_ASSERT_HELD(cl->cl_tif->tif_ifq);
 711
 712 #if CLASSQ_RIO
 713         if (q_is_rio(&cl->cl_q))
 714                 return (rio_getq(cl->cl_rio, &cl->cl_q));
 715         else
 716 #endif /* CLASSQ_RIO */
 717 #if CLASSQ_RED
 718         if (q_is_red(&cl->cl_q))
 719                 return (red_getq(cl->cl_red, &cl->cl_q));
 720         else
 721 #endif /* CLASSQ_RED */
 722 #if CLASSQ_BLUE
 723         if (q_is_blue(&cl->cl_q))
 724                 return (blue_getq(cl->cl_blue, &cl->cl_q));
 725         else
 726 #endif /* CLASSQ_BLUE */
 727         if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL)
 728                 return (sfb_getq(cl->cl_sfb, &cl->cl_q));
 729
 730         return (_getq(&cl->cl_q));
 731 }
 732
 733 static inline struct mbuf *
 734 tcq_pollq(struct tcq_class *cl)
 735 {
 736         IFCQ_LOCK_ASSERT_HELD(cl->cl_tif->tif_ifq);
 737
 738         return (qhead(&cl->cl_q));
 739 }
 740
 741 static void
 742 tcq_purgeq(struct tcq_if *tif, struct tcq_class *cl, u_int32_t flow,
 743     u_int32_t *packets, u_int32_t *bytes)
 744 {
 745         struct ifclassq *ifq = tif->tif_ifq;
 746         u_int32_t cnt = 0, len = 0, qlen;
 747
 748         IFCQ_LOCK_ASSERT_HELD(ifq);
 749
 750         if ((qlen = qlen(&cl->cl_q)) == 0)
 751                 goto done;
 752
 753         /* become regular mutex before freeing mbufs */
 754         IFCQ_CONVERT_LOCK(ifq);
 755
 756 #if CLASSQ_RIO
 757         if (q_is_rio(&cl->cl_q))
 758                 rio_purgeq(cl->cl_rio, &cl->cl_q, flow, &cnt, &len);
 759         else
 760 #endif /* CLASSQ_RIO */
 761 #if CLASSQ_RED
 762         if (q_is_red(&cl->cl_q))
 763                 red_purgeq(cl->cl_red, &cl->cl_q, flow, &cnt, &len);
 764         else
 765 #endif /* CLASSQ_RED */
 766 #if CLASSQ_BLUE
 767         if (q_is_blue(&cl->cl_q))
 768                 blue_purgeq(cl->cl_blue, &cl->cl_q, flow, &cnt, &len);
 769         else
 770 #endif /* CLASSQ_BLUE */
 771         if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL)
 772                 sfb_purgeq(cl->cl_sfb, &cl->cl_q, flow, &cnt, &len);
 773         else
 774                 _flushq_flow(&cl->cl_q, flow, &cnt, &len);
 775
 776         if (cnt > 0) {
 777                 VERIFY(qlen(&cl->cl_q) == (qlen - cnt));
 778
 779                 PKTCNTR_ADD(&cl->cl_dropcnt, cnt, len);
 780                 IFCQ_DROP_ADD(ifq, cnt, len);
 781
 782                 VERIFY(((signed)IFCQ_LEN(ifq) - cnt) >= 0);
 783                 IFCQ_LEN(ifq) -= cnt;
 784
 785                 if (pktsched_verbose) {
 786                         log(LOG_DEBUG, "%s: %s purge qid=%d pri=%d "
 787                             "qlen=[%d,%d] cnt=%d len=%d flow=0x%x\n",
 788                             if_name(TCQIF_IFP(tif)), tcq_style(tif),
 789                             cl->cl_handle, cl->cl_pri, qlen, qlen(&cl->cl_q),
 790                             cnt, len, flow);
 791                 }
 792         }
 793 done:
 794         if (packets != NULL)
 795                 *packets = cnt;
 796         if (bytes != NULL)
 797                 *bytes = len;
 798 }
 799
 800 static void
 801 tcq_updateq(struct tcq_if *tif, struct tcq_class *cl, cqev_t ev)
 802 {
 803         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 804
 805         if (pktsched_verbose) {
 806                 log(LOG_DEBUG, "%s: %s update qid=%d pri=%d event=%s\n",
 807                     if_name(TCQIF_IFP(tif)), tcq_style(tif),
 808                     cl->cl_handle, cl->cl_pri, ifclassq_ev2str(ev));
 809         }
 810
 811 #if CLASSQ_RIO
 812         if (q_is_rio(&cl->cl_q))
 813                 return (rio_updateq(cl->cl_rio, ev));
 814 #endif /* CLASSQ_RIO */
 815 #if CLASSQ_RED
 816         if (q_is_red(&cl->cl_q))
 817                 return (red_updateq(cl->cl_red, ev));
 818 #endif /* CLASSQ_RED */
 819 #if CLASSQ_BLUE
 820         if (q_is_blue(&cl->cl_q))
 821                 return (blue_updateq(cl->cl_blue, ev));
 822 #endif /* CLASSQ_BLUE */
 823         if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL)
 824                 return (sfb_updateq(cl->cl_sfb, ev));
 825 }
 826
 827 int
 828 tcq_get_class_stats(struct tcq_if *tif, u_int32_t qid,
 829     struct tcq_classstats *sp)
 830 {
 831         struct tcq_class *cl;
 832
 833         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 834
 835         if ((cl = tcq_clh_to_clp(tif, qid)) == NULL)
 836                 return (EINVAL);
 837
 838         sp->class_handle = cl->cl_handle;
 839         sp->priority = cl->cl_pri;
 840         sp->qlength = qlen(&cl->cl_q);
 841         sp->qlimit = qlimit(&cl->cl_q);
 842         sp->period = cl->cl_period;
 843         sp->xmitcnt = cl->cl_xmitcnt;
 844         sp->dropcnt = cl->cl_dropcnt;
 845
 846         sp->qtype = qtype(&cl->cl_q);
 847         sp->qstate = qstate(&cl->cl_q);
 848 #if CLASSQ_RED
 849         if (q_is_red(&cl->cl_q))
 850                 red_getstats(cl->cl_red, &sp->red[0]);
 851 #endif /* CLASSQ_RED */
 852 #if CLASSQ_RIO
 853         if (q_is_rio(&cl->cl_q))
 854                 rio_getstats(cl->cl_rio, &sp->red[0]);
 855 #endif /* CLASSQ_RIO */
 856 #if CLASSQ_BLUE
 857         if (q_is_blue(&cl->cl_q))
 858                 blue_getstats(cl->cl_blue, &sp->blue);
 859 #endif /* CLASSQ_BLUE */
 860         if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL)
 861                 sfb_getstats(cl->cl_sfb, &sp->sfb);
 862
 863         return (0);
 864 }
 865
 866 static int
 867 tcq_stat_sc(struct tcq_if *tif, cqrq_stat_sc_t *sr)
 868 {
 869         struct ifclassq *ifq = tif->tif_ifq;
 870         struct tcq_class *cl;
 871         u_int32_t i;
 872
 873         IFCQ_LOCK_ASSERT_HELD(ifq);
 874
 875         VERIFY(sr->sc == MBUF_SC_UNSPEC || MBUF_VALID_SC(sr->sc));
 876
 877         i = MBUF_SCIDX(sr->sc);
 878         VERIFY(i < IFCQ_SC_MAX);
 879
 880         cl = ifq->ifcq_disc_slots[i].cl;
 881         sr->packets = qlen(&cl->cl_q);
 882         sr->bytes = qsize(&cl->cl_q);
 883
 884         return (0);
 885 }
 886
 887 /* convert a class handle to the corresponding class pointer */
 888 static inline struct tcq_class *
 889 tcq_clh_to_clp(struct tcq_if *tif, u_int32_t chandle)
 890 {
 891         struct tcq_class *cl;
 892         int idx;
 893
 894         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 895
 896         for (idx = tif->tif_maxpri; idx >= 0; idx--)
 897                 if ((cl = tif->tif_classes[idx]) != NULL &&
 898                     cl->cl_handle == chandle)
 899                         return (cl);
 900
 901         return (NULL);
 902 }
 903
 904 static const char *
 905 tcq_style(struct tcq_if *tif)
 906 {
 907         return ((tif->tif_flags & TCQIFF_ALTQ) ? "ALTQ_TCQ" : "TCQ");
 908 }
 909
 910 /*
 911  * tcq_enqueue_ifclassq is an enqueue function to be registered to
 912  * (*ifcq_enqueue) in struct ifclassq.
 913  */
 914 static int
 915 tcq_enqueue_ifclassq(struct ifclassq *ifq, struct mbuf *m)
 916 {
 917         u_int32_t i;
 918
 919         IFCQ_LOCK_ASSERT_HELD(ifq);
 920
 921         if (!(m->m_flags & M_PKTHDR)) {
 922                 /* should not happen */
 923                 log(LOG_ERR, "%s: packet does not have pkthdr\n",
 924                     if_name(ifq->ifcq_ifp));
 925                 IFCQ_CONVERT_LOCK(ifq);
 926                 m_freem(m);
 927                 return (ENOBUFS);
 928         }
 929
 930         i = MBUF_SCIDX(mbuf_get_service_class(m));
 931         VERIFY((u_int32_t)i < IFCQ_SC_MAX);
 932
 933         return (tcq_enqueue(ifq->ifcq_disc,
 934             ifq->ifcq_disc_slots[i].cl, m, m_pftag(m)));
 935 }
 936
 937 /*
 938  * tcq_dequeue_tc_ifclassq is a dequeue function to be registered to
 939  * (*ifcq_dequeue) in struct ifclass.
 940  *
 941  * note: CLASSQDQ_POLL returns the next packet without removing the packet
 942  *      from the queue.  CLASSQDQ_REMOVE is a normal dequeue operation.
 943  *      CLASSQDQ_REMOVE must return the same packet if called immediately
 944  *      after CLASSQDQ_POLL.
 945  */
 946 static struct mbuf *
 947 tcq_dequeue_tc_ifclassq(struct ifclassq *ifq, mbuf_svc_class_t sc,
 948     cqdq_op_t op)
 949 {
 950         u_int32_t i = MBUF_SCIDX(sc);
 951
 952         VERIFY((u_int32_t)i < IFCQ_SC_MAX);
 953
 954         return (tcq_dequeue_cl(ifq->ifcq_disc,
 955             ifq->ifcq_disc_slots[i].cl, sc, op));
 956 }
 957
 958 static int
 959 tcq_request_ifclassq(struct ifclassq *ifq, cqrq_t req, void *arg)
 960 {
 961         struct tcq_if   *tif = (struct tcq_if *)ifq->ifcq_disc;
 962         int err = 0;
 963
 964         IFCQ_LOCK_ASSERT_HELD(ifq);
 965
 966         switch (req) {
 967         case CLASSQRQ_PURGE:
 968                 tcq_purge(tif);
 969                 break;
 970
 971         case CLASSQRQ_PURGE_SC:
 972                 tcq_purge_sc(tif, (cqrq_purge_sc_t *)arg);
 973                 break;
 974
 975         case CLASSQRQ_EVENT:
 976                 tcq_event(tif, (cqev_t)arg);
 977                 break;
 978
 979         case CLASSQRQ_THROTTLE:
 980                 err = tcq_throttle(tif, (cqrq_throttle_t *)arg);
 981                 break;
 982
 983         case CLASSQRQ_STAT_SC:
 984                 err = tcq_stat_sc(tif, (cqrq_stat_sc_t *)arg);
 985                 break;
 986         }
 987         return (err);
 988 }
 989
 990 int
 991 tcq_setup_ifclassq(struct ifclassq *ifq, u_int32_t flags)
 992 {
 993         struct ifnet *ifp = ifq->ifcq_ifp;
 994         struct tcq_class *cl0, *cl1, *cl2, *cl3;
 995         struct tcq_if *tif;
 996         u_int32_t maxlen = 0, qflags = 0;
 997         int err = 0;
 998
 999         IFCQ_LOCK_ASSERT_HELD(ifq);
1000         VERIFY(ifq->ifcq_disc == NULL);
1001         VERIFY(ifq->ifcq_type == PKTSCHEDT_NONE);
1002
1003         if (flags & PKTSCHEDF_QALG_RED)
1004                 qflags |= TQCF_RED;
1005         if (flags & PKTSCHEDF_QALG_RIO)
1006                 qflags |= TQCF_RIO;
1007         if (flags & PKTSCHEDF_QALG_BLUE)
1008                 qflags |= TQCF_BLUE;
1009         if (flags & PKTSCHEDF_QALG_SFB)
1010                 qflags |= TQCF_SFB;
1011         if (flags & PKTSCHEDF_QALG_ECN)
1012                 qflags |= TQCF_ECN;
1013         if (flags & PKTSCHEDF_QALG_FLOWCTL)
1014                 qflags |= TQCF_FLOWCTL;
1015
1016         tif = tcq_alloc(ifp, M_WAITOK, FALSE);
1017         if (tif == NULL)
1018                 return (ENOMEM);
1019
1020         if ((maxlen = IFCQ_MAXLEN(ifq)) == 0)
1021                 maxlen = if_sndq_maxlen;
1022
1023         if ((err = tcq_add_queue(tif, 0, maxlen,
1024             qflags | PRCF_LAZY, SCIDX_BK, &cl0)) != 0)
1025                 goto cleanup;
1026
1027         if ((err = tcq_add_queue(tif, 1, maxlen,
1028             qflags | TQCF_DEFAULTCLASS, SCIDX_BE, &cl1)) != 0)
1029                 goto cleanup;
1030
1031         if ((err = tcq_add_queue(tif, 2, maxlen,
1032             qflags | PRCF_LAZY, SCIDX_VI, &cl2)) != 0)
1033                 goto cleanup;
1034
1035         if ((err = tcq_add_queue(tif, 3, maxlen,
1036             qflags, SCIDX_VO, &cl3)) != 0)
1037                 goto cleanup;
1038
1039         err = ifclassq_attach(ifq, PKTSCHEDT_TCQ, tif,
1040             tcq_enqueue_ifclassq, NULL, tcq_dequeue_tc_ifclassq,
1041             tcq_request_ifclassq);
1042
1043         /* cache these for faster lookup */
1044         if (err == 0) {
1045                 /* Map {BK_SYS,BK} to TC_BK */
1046                 ifq->ifcq_disc_slots[SCIDX_BK_SYS].qid = SCIDX_BK;
1047                 ifq->ifcq_disc_slots[SCIDX_BK_SYS].cl = cl0;
1048
1049                 ifq->ifcq_disc_slots[SCIDX_BK].qid = SCIDX_BK;
1050                 ifq->ifcq_disc_slots[SCIDX_BK].cl = cl0;
1051
1052                 /* Map {BE,RD,OAM} to TC_BE */
1053                 ifq->ifcq_disc_slots[SCIDX_BE].qid = SCIDX_BE;
1054                 ifq->ifcq_disc_slots[SCIDX_BE].cl = cl1;
1055
1056                 ifq->ifcq_disc_slots[SCIDX_RD].qid = SCIDX_BE;
1057                 ifq->ifcq_disc_slots[SCIDX_RD].cl = cl1;
1058
1059                 ifq->ifcq_disc_slots[SCIDX_OAM].qid = SCIDX_BE;
1060                 ifq->ifcq_disc_slots[SCIDX_OAM].cl = cl1;
1061
1062                 /* Map {AV,RV,VI} to TC_VI */
1063                 ifq->ifcq_disc_slots[SCIDX_AV].qid = SCIDX_VI;
1064                 ifq->ifcq_disc_slots[SCIDX_AV].cl = cl2;
1065
1066                 ifq->ifcq_disc_slots[SCIDX_RV].qid = SCIDX_VI;
1067                 ifq->ifcq_disc_slots[SCIDX_RV].cl = cl2;
1068
1069                 ifq->ifcq_disc_slots[SCIDX_VI].qid = SCIDX_VI;
1070                 ifq->ifcq_disc_slots[SCIDX_VI].cl = cl2;
1071
1072                 /* Map {VO,CTL} to TC_VO */
1073                 ifq->ifcq_disc_slots[SCIDX_VO].qid = SCIDX_VO;
1074                 ifq->ifcq_disc_slots[SCIDX_VO].cl = cl3;
1075
1076                 ifq->ifcq_disc_slots[SCIDX_CTL].qid = SCIDX_VO;
1077                 ifq->ifcq_disc_slots[SCIDX_CTL].cl = cl3;
1078         }
1079
1080 cleanup:
1081         if (err != 0)
1082                 (void) tcq_destroy_locked(tif);
1083
1084         return (err);
1085 }
1086
1087 int
1088 tcq_teardown_ifclassq(struct ifclassq *ifq)
1089 {
1090         struct tcq_if *tif = ifq->ifcq_disc;
1091         int i;
1092
1093         IFCQ_LOCK_ASSERT_HELD(ifq);
1094         VERIFY(tif != NULL && ifq->ifcq_type == PKTSCHEDT_TCQ);
1095
1096         (void) tcq_destroy_locked(tif);
1097
1098         ifq->ifcq_disc = NULL;
1099         for (i = 0; i < IFCQ_SC_MAX; i++) {
1100                 ifq->ifcq_disc_slots[i].qid = 0;
1101                 ifq->ifcq_disc_slots[i].cl = NULL;
1102         }
1103
1104         return (ifclassq_detach(ifq));
1105 }
1106
1107 int
1108 tcq_getqstats_ifclassq(struct ifclassq *ifq, u_int32_t slot,
1109     struct if_ifclassq_stats *ifqs)
1110 {
1111         struct tcq_if *tif = ifq->ifcq_disc;
1112
1113         IFCQ_LOCK_ASSERT_HELD(ifq);
1114         VERIFY(ifq->ifcq_type == PKTSCHEDT_TCQ);
1115
1116         if (slot >= IFCQ_SC_MAX)
1117                 return (EINVAL);
1118
1119         return (tcq_get_class_stats(tif, ifq->ifcq_disc_slots[slot].qid,
1120             &ifqs->ifqs_tcq_stats));
1121 }
1122
1123 static int
1124 tcq_throttle(struct tcq_if *tif, cqrq_throttle_t *tr)
1125 {
1126         struct ifclassq *ifq = tif->tif_ifq;
1127         struct tcq_class *cl;
1128         int err = 0;
1129
1130         IFCQ_LOCK_ASSERT_HELD(ifq);
1131         VERIFY(!(tif->tif_flags & TCQIFF_ALTQ));
1132
1133         if (!tr->set) {
1134                 tr->level = tif->tif_throttle;
1135                 return (0);
1136         }
1137
1138         if (tr->level == tif->tif_throttle)
1139                 return (EALREADY);
1140
1141         /* Current throttling levels only involve BK_SYS class */
1142         cl = ifq->ifcq_disc_slots[SCIDX_BK_SYS].cl;
1143
1144         switch (tr->level) {
1145         case IFNET_THROTTLE_OFF:
1146                 err = tcq_resumeq(tif, cl);
1147                 break;
1148
1149         case IFNET_THROTTLE_OPPORTUNISTIC:
1150                 err = tcq_suspendq(tif, cl);
1151                 break;
1152
1153         default:
1154                 VERIFY(0);
1155                 /* NOTREACHED */
1156         }
1157
1158         if (err == 0 || err == ENXIO) {
1159                 if (pktsched_verbose) {
1160                         log(LOG_DEBUG, "%s: %s throttling %slevel set %d->%d\n",
1161                             if_name(TCQIF_IFP(tif)), tcq_style(tif),
1162                             (err == 0) ? "" : "lazy ", tif->tif_throttle,
1163                             tr->level);
1164                 }
1165                 tif->tif_throttle = tr->level;
1166                 if (err != 0)
1167                         err = 0;
1168                 else
1169                         tcq_purgeq(tif, cl, 0, NULL, NULL);
1170         } else {
1171                 log(LOG_ERR, "%s: %s unable to set throttling level "
1172                     "%d->%d [error=%d]\n", if_name(TCQIF_IFP(tif)),
1173                     tcq_style(tif), tif->tif_throttle, tr->level, err);
1174         }
1175
1176         return (err);
1177 }
1178
1179 static int
1180 tcq_resumeq(struct tcq_if *tif, struct tcq_class *cl)
1181 {
1182         struct ifclassq *ifq = tif->tif_ifq;
1183         int err = 0;
1184
1185         IFCQ_LOCK_ASSERT_HELD(ifq);
1186
1187 #if CLASSQ_RIO
1188         if (q_is_rio(&cl->cl_q))
1189                 err = rio_suspendq(cl->cl_rio, &cl->cl_q, FALSE);
1190         else
1191 #endif /* CLASSQ_RIO */
1192 #if CLASSQ_RED
1193         if (q_is_red(&cl->cl_q))
1194                 err = red_suspendq(cl->cl_red, &cl->cl_q, FALSE);
1195         else
1196 #endif /* CLASSQ_RED */
1197 #if CLASSQ_BLUE
1198         if (q_is_blue(&cl->cl_q))
1199                 err = blue_suspendq(cl->cl_blue, &cl->cl_q, FALSE);
1200         else
1201 #endif /* CLASSQ_BLUE */
1202         if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL)
1203                 err = sfb_suspendq(cl->cl_sfb, &cl->cl_q, FALSE);
1204
1205         if (err == 0)
1206                 qstate(&cl->cl_q) = QS_RUNNING;
1207
1208         return (err);
1209 }
1210
1211 static int
1212 tcq_suspendq(struct tcq_if *tif, struct tcq_class *cl)
1213 {
1214         struct ifclassq *ifq = tif->tif_ifq;
1215         int err = 0;
1216
1217         IFCQ_LOCK_ASSERT_HELD(ifq);
1218
1219 #if CLASSQ_RIO
1220         if (q_is_rio(&cl->cl_q))
1221                 err = rio_suspendq(cl->cl_rio, &cl->cl_q, TRUE);
1222         else
1223 #endif /* CLASSQ_RIO */
1224 #if CLASSQ_RED
1225         if (q_is_red(&cl->cl_q))
1226                 err = red_suspendq(cl->cl_red, &cl->cl_q, TRUE);
1227         else
1228 #endif /* CLASSQ_RED */
1229 #if CLASSQ_BLUE
1230         if (q_is_blue(&cl->cl_q))
1231                 err = blue_suspendq(cl->cl_blue, &cl->cl_q, TRUE);
1232         else
1233 #endif /* CLASSQ_BLUE */
1234         if (q_is_sfb(&cl->cl_q)) {
1235                 if (cl->cl_sfb != NULL) {
1236                         err = sfb_suspendq(cl->cl_sfb, &cl->cl_q, TRUE);
1237                 } else {
1238                         VERIFY(cl->cl_flags & TQCF_LAZY);
1239                         err = ENXIO;    /* delayed throttling */
1240                 }
1241         }
1242
1243         if (err == 0 || err == ENXIO)
1244                 qstate(&cl->cl_q) = QS_SUSPENDED;
1245
1246         return (err);
1247 }