bsd/net/pktsched/pktsched_tcq.c

   1 /*
   2  * Copyright (c) 2011-2016 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 /*
  52  * function prototypes
  53  */
  54 static int tcq_enqueue_ifclassq(struct ifclassq *, void *, classq_pkt_type_t,
  55     boolean_t *);
  56 static void *tcq_dequeue_tc_ifclassq(struct ifclassq *, mbuf_svc_class_t,
  57     classq_pkt_type_t *);
  58 static int tcq_request_ifclassq(struct ifclassq *, cqrq_t, void *);
  59 static int tcq_clear_interface(struct tcq_if *);
  60 static struct tcq_class *tcq_class_create(struct tcq_if *, int, u_int32_t,
  61     int, u_int32_t, classq_pkt_type_t);
  62 static int tcq_class_destroy(struct tcq_if *, struct tcq_class *);
  63 static int tcq_destroy_locked(struct tcq_if *);
  64 static inline int tcq_addq(struct tcq_class *, pktsched_pkt_t *,
  65     struct pf_mtag *);
  66 static inline void tcq_getq(struct tcq_class *, pktsched_pkt_t *);
  67 static void tcq_purgeq(struct tcq_if *, struct tcq_class *, u_int32_t,
  68     u_int32_t *, u_int32_t *);
  69 static void tcq_purge_sc(struct tcq_if *, cqrq_purge_sc_t *);
  70 static void tcq_updateq(struct tcq_if *, struct tcq_class *, cqev_t);
  71 static int tcq_throttle(struct tcq_if *, cqrq_throttle_t *);
  72 static int tcq_resumeq(struct tcq_if *, struct tcq_class *);
  73 static int tcq_suspendq(struct tcq_if *, struct tcq_class *);
  74 static int tcq_stat_sc(struct tcq_if *, cqrq_stat_sc_t *);
  75 static void tcq_dequeue_cl(struct tcq_if *, struct tcq_class *,
  76     mbuf_svc_class_t, pktsched_pkt_t *);
  77 static inline struct tcq_class *tcq_clh_to_clp(struct tcq_if *, u_int32_t);
  78 static const char *tcq_style(struct tcq_if *);
  79
  80 #define TCQ_ZONE_MAX    32              /* maximum elements in zone */
  81 #define TCQ_ZONE_NAME   "pktsched_tcq"  /* zone name */
  82
  83 static unsigned int tcq_size;           /* size of zone element */
  84 static struct zone *tcq_zone;           /* zone for tcq */
  85
  86 #define TCQ_CL_ZONE_MAX 32              /* maximum elements in zone */
  87 #define TCQ_CL_ZONE_NAME "pktsched_tcq_cl" /* zone name */
  88
  89 static unsigned int tcq_cl_size;        /* size of zone element */
  90 static struct zone *tcq_cl_zone;        /* zone for tcq_class */
  91
  92 void
  93 tcq_init(void)
  94 {
  95         tcq_size = sizeof (struct tcq_if);
  96         tcq_zone = zinit(tcq_size, TCQ_ZONE_MAX * tcq_size,
  97             0, TCQ_ZONE_NAME);
  98         if (tcq_zone == NULL) {
  99                 panic("%s: failed allocating %s", __func__, TCQ_ZONE_NAME);
 100                 /* NOTREACHED */
 101         }
 102         zone_change(tcq_zone, Z_EXPAND, TRUE);
 103         zone_change(tcq_zone, Z_CALLERACCT, TRUE);
 104
 105         tcq_cl_size = sizeof (struct tcq_class);
 106         tcq_cl_zone = zinit(tcq_cl_size, TCQ_CL_ZONE_MAX * tcq_cl_size,
 107             0, TCQ_CL_ZONE_NAME);
 108         if (tcq_cl_zone == NULL) {
 109                 panic("%s: failed allocating %s", __func__, TCQ_CL_ZONE_NAME);
 110                 /* NOTREACHED */
 111         }
 112         zone_change(tcq_cl_zone, Z_EXPAND, TRUE);
 113         zone_change(tcq_cl_zone, Z_CALLERACCT, TRUE);
 114 }
 115
 116 struct tcq_if *
 117 tcq_alloc(struct ifnet *ifp, int how)
 118 {
 119         struct tcq_if   *tif;
 120
 121         tif = (how == M_WAITOK) ? zalloc(tcq_zone) : zalloc_noblock(tcq_zone);
 122         if (tif == NULL)
 123                 return (NULL);
 124
 125         bzero(tif, tcq_size);
 126         tif->tif_maxpri = -1;
 127         tif->tif_ifq = &ifp->if_snd;
 128
 129         if (pktsched_verbose) {
 130                 log(LOG_DEBUG, "%s: %s scheduler allocated\n",
 131                     if_name(ifp), tcq_style(tif));
 132         }
 133
 134         return (tif);
 135 }
 136
 137 int
 138 tcq_destroy(struct tcq_if *tif)
 139 {
 140         struct ifclassq *ifq = tif->tif_ifq;
 141         int err;
 142
 143         IFCQ_LOCK(ifq);
 144         err = tcq_destroy_locked(tif);
 145         IFCQ_UNLOCK(ifq);
 146
 147         return (err);
 148 }
 149
 150 static int
 151 tcq_destroy_locked(struct tcq_if *tif)
 152 {
 153         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 154
 155         (void) tcq_clear_interface(tif);
 156
 157         if (pktsched_verbose) {
 158                 log(LOG_DEBUG, "%s: %s scheduler destroyed\n",
 159                     if_name(TCQIF_IFP(tif)), tcq_style(tif));
 160         }
 161
 162         zfree(tcq_zone, tif);
 163
 164         return (0);
 165 }
 166
 167 /*
 168  * bring the interface back to the initial state by discarding
 169  * all the filters and classes.
 170  */
 171 static int
 172 tcq_clear_interface(struct tcq_if *tif)
 173 {
 174         struct tcq_class        *cl;
 175         int pri;
 176
 177         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 178
 179         /* clear out the classes */
 180         for (pri = 0; pri <= tif->tif_maxpri; pri++)
 181                 if ((cl = tif->tif_classes[pri]) != NULL)
 182                         tcq_class_destroy(tif, cl);
 183
 184         return (0);
 185 }
 186
 187 /* discard all the queued packets on the interface */
 188 void
 189 tcq_purge(struct tcq_if *tif)
 190 {
 191         struct tcq_class *cl;
 192         int pri;
 193
 194         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 195
 196         for (pri = 0; pri <= tif->tif_maxpri; pri++) {
 197                 if ((cl = tif->tif_classes[pri]) != NULL && !qempty(&cl->cl_q))
 198                         tcq_purgeq(tif, cl, 0, NULL, NULL);
 199         }
 200         VERIFY(IFCQ_LEN(tif->tif_ifq) == 0);
 201 }
 202
 203 static void
 204 tcq_purge_sc(struct tcq_if *tif, cqrq_purge_sc_t *pr)
 205 {
 206         struct ifclassq *ifq = tif->tif_ifq;
 207         u_int32_t i;
 208
 209         IFCQ_LOCK_ASSERT_HELD(ifq);
 210
 211         VERIFY(pr->sc == MBUF_SC_UNSPEC || MBUF_VALID_SC(pr->sc));
 212         VERIFY(pr->flow != 0);
 213
 214         if (pr->sc != MBUF_SC_UNSPEC) {
 215                 i = MBUF_SCIDX(pr->sc);
 216                 VERIFY(i < IFCQ_SC_MAX);
 217
 218                 tcq_purgeq(tif, ifq->ifcq_disc_slots[i].cl,
 219                     pr->flow, &pr->packets, &pr->bytes);
 220         } else {
 221                 u_int32_t cnt, len;
 222
 223                 pr->packets = 0;
 224                 pr->bytes = 0;
 225
 226                 for (i = 0; i < IFCQ_SC_MAX; i++) {
 227                         tcq_purgeq(tif, ifq->ifcq_disc_slots[i].cl,
 228                             pr->flow, &cnt, &len);
 229                         pr->packets += cnt;
 230                         pr->bytes += len;
 231                 }
 232         }
 233 }
 234
 235 void
 236 tcq_event(struct tcq_if *tif, cqev_t ev)
 237 {
 238         struct tcq_class *cl;
 239         int pri;
 240
 241         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 242
 243         for (pri = 0; pri <= tif->tif_maxpri; pri++)
 244                 if ((cl = tif->tif_classes[pri]) != NULL)
 245                         tcq_updateq(tif, cl, ev);
 246 }
 247
 248 int
 249 tcq_add_queue(struct tcq_if *tif, int priority, u_int32_t qlimit,
 250     int flags, u_int32_t qid, struct tcq_class **clp, classq_pkt_type_t ptype)
 251 {
 252         struct tcq_class *cl;
 253
 254         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 255
 256         /* check parameters */
 257         if (priority >= TCQ_MAXPRI)
 258                 return (EINVAL);
 259         if (tif->tif_classes[priority] != NULL)
 260                 return (EBUSY);
 261         if (tcq_clh_to_clp(tif, qid) != NULL)
 262                 return (EBUSY);
 263
 264         cl = tcq_class_create(tif, priority, qlimit, flags, qid, ptype);
 265         if (cl == NULL)
 266                 return (ENOMEM);
 267
 268         if (clp != NULL)
 269                 *clp = cl;
 270
 271         return (0);
 272 }
 273
 274 static struct tcq_class *
 275 tcq_class_create(struct tcq_if *tif, int pri, u_int32_t qlimit,
 276     int flags, u_int32_t qid, classq_pkt_type_t ptype)
 277 {
 278         struct ifnet *ifp;
 279         struct ifclassq *ifq;
 280         struct tcq_class *cl;
 281
 282         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 283
 284         ifq = tif->tif_ifq;
 285         ifp = TCQIF_IFP(tif);
 286
 287         if ((cl = tif->tif_classes[pri]) != NULL) {
 288                 /* modify the class instead of creating a new one */
 289                 if (!qempty(&cl->cl_q))
 290                         tcq_purgeq(tif, cl, 0, NULL, NULL);
 291
 292                 if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL)
 293                         sfb_destroy(cl->cl_sfb);
 294                 cl->cl_qalg.ptr = NULL;
 295                 qtype(&cl->cl_q) = Q_DROPTAIL;
 296                 qstate(&cl->cl_q) = QS_RUNNING;
 297                 VERIFY(qptype(&cl->cl_q) == ptype);
 298         } else {
 299                 cl = zalloc(tcq_cl_zone);
 300                 if (cl == NULL)
 301                         return (NULL);
 302
 303                 bzero(cl, tcq_cl_size);
 304         }
 305
 306         tif->tif_classes[pri] = cl;
 307         if (flags & TQCF_DEFAULTCLASS)
 308                 tif->tif_default = cl;
 309         if (qlimit == 0 || qlimit > IFCQ_MAXLEN(ifq)) {
 310                 qlimit = IFCQ_MAXLEN(ifq);
 311                 if (qlimit == 0)
 312                         qlimit = DEFAULT_QLIMIT;  /* use default */
 313         }
 314         _qinit(&cl->cl_q, Q_DROPTAIL, qlimit, ptype);
 315         cl->cl_flags = flags;
 316         cl->cl_pri = pri;
 317         if (pri > tif->tif_maxpri)
 318                 tif->tif_maxpri = pri;
 319         cl->cl_tif = tif;
 320         cl->cl_handle = qid;
 321
 322         if (flags & TQCF_SFB) {
 323                 cl->cl_qflags = 0;
 324                 if (flags & TQCF_ECN) {
 325                         cl->cl_qflags |= SFBF_ECN;
 326                 }
 327                 if (flags & TQCF_FLOWCTL) {
 328                         cl->cl_qflags |= SFBF_FLOWCTL;
 329                 }
 330                 if (flags & TQCF_DELAYBASED) {
 331                         cl->cl_qflags |= SFBF_DELAYBASED;
 332                 }
 333                 if (!(cl->cl_flags & TQCF_LAZY))
 334                         cl->cl_sfb = sfb_alloc(ifp, cl->cl_handle,
 335                             qlimit(&cl->cl_q), cl->cl_qflags);
 336                 if (cl->cl_sfb != NULL || (cl->cl_flags & TQCF_LAZY))
 337                         qtype(&cl->cl_q) = Q_SFB;
 338         }
 339
 340         if (pktsched_verbose) {
 341                 log(LOG_DEBUG, "%s: %s created qid=%d pri=%d qlimit=%d "
 342                     "flags=%b\n", if_name(ifp), tcq_style(tif),
 343                     cl->cl_handle, cl->cl_pri, qlimit, flags, TQCF_BITS);
 344         }
 345
 346         return (cl);
 347 }
 348
 349 int
 350 tcq_remove_queue(struct tcq_if *tif, u_int32_t qid)
 351 {
 352         struct tcq_class *cl;
 353
 354         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 355
 356         if ((cl = tcq_clh_to_clp(tif, qid)) == NULL)
 357                 return (EINVAL);
 358
 359         return (tcq_class_destroy(tif, cl));
 360 }
 361
 362 static int
 363 tcq_class_destroy(struct tcq_if *tif, struct tcq_class *cl)
 364 {
 365         struct ifclassq *ifq = tif->tif_ifq;
 366         int pri;
 367 #if !MACH_ASSERT
 368 #pragma unused(ifq)
 369 #endif
 370         IFCQ_LOCK_ASSERT_HELD(ifq);
 371
 372         if (!qempty(&cl->cl_q))
 373                 tcq_purgeq(tif, cl, 0, NULL, NULL);
 374
 375         tif->tif_classes[cl->cl_pri] = NULL;
 376         if (tif->tif_maxpri == cl->cl_pri) {
 377                 for (pri = cl->cl_pri; pri >= 0; pri--)
 378                         if (tif->tif_classes[pri] != NULL) {
 379                                 tif->tif_maxpri = pri;
 380                                 break;
 381                         }
 382                 if (pri < 0)
 383                         tif->tif_maxpri = -1;
 384         }
 385
 386         if (tif->tif_default == cl)
 387                 tif->tif_default = NULL;
 388
 389         if (cl->cl_qalg.ptr != NULL) {
 390                 if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL)
 391                         sfb_destroy(cl->cl_sfb);
 392                 cl->cl_qalg.ptr = NULL;
 393                 qtype(&cl->cl_q) = Q_DROPTAIL;
 394                 qstate(&cl->cl_q) = QS_RUNNING;
 395         }
 396
 397         if (pktsched_verbose) {
 398                 log(LOG_DEBUG, "%s: %s destroyed qid=%d pri=%d\n",
 399                     if_name(TCQIF_IFP(tif)), tcq_style(tif),
 400                     cl->cl_handle, cl->cl_pri);
 401         }
 402
 403         zfree(tcq_cl_zone, cl);
 404         return (0);
 405 }
 406
 407 int
 408 tcq_enqueue(struct tcq_if *tif, struct tcq_class *cl, pktsched_pkt_t *pkt,
 409     struct pf_mtag *t)
 410 {
 411         struct ifclassq *ifq = tif->tif_ifq;
 412         int len, ret;
 413
 414         IFCQ_LOCK_ASSERT_HELD(ifq);
 415         VERIFY(cl == NULL || cl->cl_tif == tif);
 416
 417         if (cl == NULL) {
 418                 cl = tcq_clh_to_clp(tif, 0);
 419                 if (cl == NULL) {
 420                         cl = tif->tif_default;
 421                         if (cl == NULL) {
 422                                 IFCQ_CONVERT_LOCK(ifq);
 423                                 return (CLASSQEQ_DROP);
 424                         }
 425                 }
 426         }
 427
 428         VERIFY(pkt->pktsched_ptype == qptype(&cl->cl_q));
 429         len = pktsched_get_pkt_len(pkt);
 430
 431         ret = tcq_addq(cl, pkt, t);
 432         if ((ret != 0) && (ret != CLASSQEQ_SUCCESS_FC)) {
 433                 VERIFY(ret == CLASSQEQ_DROP ||
 434                     ret == CLASSQEQ_DROP_FC ||
 435                     ret == CLASSQEQ_DROP_SP);
 436                 PKTCNTR_ADD(&cl->cl_dropcnt, 1, len);
 437                 IFCQ_DROP_ADD(ifq, 1, len);
 438                 return (ret);
 439         }
 440         IFCQ_INC_LEN(ifq);
 441         IFCQ_INC_BYTES(ifq, len);
 442
 443         /* successfully queued. */
 444         return (ret);
 445 }
 446
 447 /*
 448  * note: CLASSQDQ_POLL returns the next packet without removing the packet
 449  *      from the queue.  CLASSQDQ_REMOVE is a normal dequeue operation.
 450  *      CLASSQDQ_REMOVE must return the same packet if called immediately
 451  *      after CLASSQDQ_POLL.
 452  */
 453 void
 454 tcq_dequeue_tc(struct tcq_if *tif, mbuf_svc_class_t sc, pktsched_pkt_t *pkt)
 455 {
 456         tcq_dequeue_cl(tif, NULL, sc, pkt);
 457 }
 458
 459 static void
 460 tcq_dequeue_cl(struct tcq_if *tif, struct tcq_class *cl, mbuf_svc_class_t sc,
 461     pktsched_pkt_t *pkt)
 462 {
 463         struct ifclassq *ifq = tif->tif_ifq;
 464         uint32_t len;
 465
 466         IFCQ_LOCK_ASSERT_HELD(ifq);
 467
 468         if (cl == NULL) {
 469                 cl = tcq_clh_to_clp(tif, MBUF_SCIDX(sc));
 470                 if (cl == NULL) {
 471                         pkt->pktsched_pkt = NULL;
 472                         return;
 473                 }
 474         }
 475
 476         if (qempty(&cl->cl_q)) {
 477                 pkt->pktsched_pkt = NULL;
 478                 return;
 479         }
 480
 481         VERIFY(!IFCQ_IS_EMPTY(ifq));
 482
 483         tcq_getq(cl, pkt);
 484         if (pkt->pktsched_pkt != NULL) {
 485                 len = pktsched_get_pkt_len(pkt);
 486                 IFCQ_DEC_LEN(ifq);
 487                 IFCQ_DEC_BYTES(ifq, len);
 488                 if (qempty(&cl->cl_q))
 489                         cl->cl_period++;
 490                 PKTCNTR_ADD(&cl->cl_xmitcnt, 1, len);
 491                 IFCQ_XMIT_ADD(ifq, 1, len);
 492         }
 493 }
 494
 495 static inline int
 496 tcq_addq(struct tcq_class *cl, pktsched_pkt_t *pkt, struct pf_mtag *t)
 497 {
 498         struct tcq_if *tif = cl->cl_tif;
 499         struct ifclassq *ifq = tif->tif_ifq;
 500
 501         IFCQ_LOCK_ASSERT_HELD(ifq);
 502
 503         if (q_is_sfb(&cl->cl_q)) {
 504                 if (cl->cl_sfb == NULL) {
 505                         struct ifnet *ifp = TCQIF_IFP(tif);
 506
 507                         VERIFY(cl->cl_flags & TQCF_LAZY);
 508                         cl->cl_flags &= ~TQCF_LAZY;
 509                         IFCQ_CONVERT_LOCK(ifq);
 510
 511                         cl->cl_sfb = sfb_alloc(ifp, cl->cl_handle,
 512                             qlimit(&cl->cl_q), cl->cl_qflags);
 513                         if (cl->cl_sfb == NULL) {
 514                                 /* fall back to droptail */
 515                                 qtype(&cl->cl_q) = Q_DROPTAIL;
 516                                 cl->cl_flags &= ~TQCF_SFB;
 517                                 cl->cl_qflags &= ~(SFBF_ECN | SFBF_FLOWCTL);
 518
 519                                 log(LOG_ERR, "%s: %s SFB lazy allocation "
 520                                     "failed for qid=%d pri=%d, falling back "
 521                                     "to DROPTAIL\n", if_name(ifp),
 522                                     tcq_style(tif), cl->cl_handle,
 523                                     cl->cl_pri);
 524                         } else if (tif->tif_throttle != IFNET_THROTTLE_OFF) {
 525                                 /* if there's pending throttling, set it */
 526                                 cqrq_throttle_t tr = { 1, tif->tif_throttle };
 527                                 int err = tcq_throttle(tif, &tr);
 528
 529                                 if (err == EALREADY)
 530                                         err = 0;
 531                                 if (err != 0) {
 532                                         tr.level = IFNET_THROTTLE_OFF;
 533                                         (void) tcq_throttle(tif, &tr);
 534                                 }
 535                         }
 536                 }
 537                 if (cl->cl_sfb != NULL)
 538                         return (sfb_addq(cl->cl_sfb, &cl->cl_q, pkt, t));
 539         } else if (qlen(&cl->cl_q) >= qlimit(&cl->cl_q)) {
 540                 IFCQ_CONVERT_LOCK(ifq);
 541                 return (CLASSQEQ_DROP);
 542         }
 543
 544 #if PF_ECN
 545         if (cl->cl_flags & TQCF_CLEARDSCP)
 546                 /* not supported for skywalk packets */
 547                 VERIFY(pkt->pktsched_ptype == QP_MBUF);
 548                 write_dsfield(m, t, 0);
 549 #endif /* PF_ECN */
 550
 551         VERIFY(pkt->pktsched_ptype == qptype(&cl->cl_q));
 552         _addq(&cl->cl_q, pkt->pktsched_pkt);
 553
 554         return (0);
 555 }
 556
 557 static inline void
 558 tcq_getq(struct tcq_class *cl, pktsched_pkt_t *pkt)
 559 {
 560         IFCQ_LOCK_ASSERT_HELD(cl->cl_tif->tif_ifq);
 561
 562         if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL) {
 563                 return (sfb_getq(cl->cl_sfb, &cl->cl_q, pkt));
 564         }
 565
 566         return (pktsched_pkt_encap(pkt, qptype(&cl->cl_q), _getq(&cl->cl_q)));
 567 }
 568
 569 static void
 570 tcq_purgeq(struct tcq_if *tif, struct tcq_class *cl, u_int32_t flow,
 571     u_int32_t *packets, u_int32_t *bytes)
 572 {
 573         struct ifclassq *ifq = tif->tif_ifq;
 574         u_int32_t cnt = 0, len = 0, qlen;
 575
 576         IFCQ_LOCK_ASSERT_HELD(ifq);
 577
 578         if ((qlen = qlen(&cl->cl_q)) == 0)
 579                 goto done;
 580
 581         IFCQ_CONVERT_LOCK(ifq);
 582         if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL)
 583                 sfb_purgeq(cl->cl_sfb, &cl->cl_q, flow, &cnt, &len);
 584         else
 585                 _flushq_flow(&cl->cl_q, flow, &cnt, &len);
 586
 587         if (cnt > 0) {
 588                 VERIFY(qlen(&cl->cl_q) == (qlen - cnt));
 589
 590                 PKTCNTR_ADD(&cl->cl_dropcnt, cnt, len);
 591                 IFCQ_DROP_ADD(ifq, cnt, len);
 592
 593                 VERIFY(((signed)IFCQ_LEN(ifq) - cnt) >= 0);
 594                 IFCQ_LEN(ifq) -= cnt;
 595
 596                 if (pktsched_verbose) {
 597                         log(LOG_DEBUG, "%s: %s purge qid=%d pri=%d "
 598                             "qlen=[%d,%d] cnt=%d len=%d flow=0x%x\n",
 599                             if_name(TCQIF_IFP(tif)), tcq_style(tif),
 600                             cl->cl_handle, cl->cl_pri, qlen, qlen(&cl->cl_q),
 601                             cnt, len, flow);
 602                 }
 603         }
 604 done:
 605         if (packets != NULL)
 606                 *packets = cnt;
 607         if (bytes != NULL)
 608                 *bytes = len;
 609 }
 610
 611 static void
 612 tcq_updateq(struct tcq_if *tif, struct tcq_class *cl, cqev_t ev)
 613 {
 614         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 615
 616         if (pktsched_verbose) {
 617                 log(LOG_DEBUG, "%s: %s update qid=%d pri=%d event=%s\n",
 618                     if_name(TCQIF_IFP(tif)), tcq_style(tif),
 619                     cl->cl_handle, cl->cl_pri, ifclassq_ev2str(ev));
 620         }
 621
 622         if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL)
 623                 return (sfb_updateq(cl->cl_sfb, ev));
 624 }
 625
 626 int
 627 tcq_get_class_stats(struct tcq_if *tif, u_int32_t qid,
 628     struct tcq_classstats *sp)
 629 {
 630         struct tcq_class *cl;
 631
 632         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 633
 634         if ((cl = tcq_clh_to_clp(tif, qid)) == NULL)
 635                 return (EINVAL);
 636
 637         sp->class_handle = cl->cl_handle;
 638         sp->priority = cl->cl_pri;
 639         sp->qlength = qlen(&cl->cl_q);
 640         sp->qlimit = qlimit(&cl->cl_q);
 641         sp->period = cl->cl_period;
 642         sp->xmitcnt = cl->cl_xmitcnt;
 643         sp->dropcnt = cl->cl_dropcnt;
 644
 645         sp->qtype = qtype(&cl->cl_q);
 646         sp->qstate = qstate(&cl->cl_q);
 647
 648         if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL)
 649                 sfb_getstats(cl->cl_sfb, &sp->sfb);
 650
 651         return (0);
 652 }
 653
 654 static int
 655 tcq_stat_sc(struct tcq_if *tif, cqrq_stat_sc_t *sr)
 656 {
 657         struct ifclassq *ifq = tif->tif_ifq;
 658         struct tcq_class *cl;
 659         u_int32_t i;
 660
 661         IFCQ_LOCK_ASSERT_HELD(ifq);
 662
 663         VERIFY(sr->sc == MBUF_SC_UNSPEC || MBUF_VALID_SC(sr->sc));
 664
 665         i = MBUF_SCIDX(sr->sc);
 666         VERIFY(i < IFCQ_SC_MAX);
 667
 668         cl = ifq->ifcq_disc_slots[i].cl;
 669         sr->packets = qlen(&cl->cl_q);
 670         sr->bytes = qsize(&cl->cl_q);
 671
 672         return (0);
 673 }
 674
 675 /* convert a class handle to the corresponding class pointer */
 676 static inline struct tcq_class *
 677 tcq_clh_to_clp(struct tcq_if *tif, u_int32_t chandle)
 678 {
 679         struct tcq_class *cl;
 680         int idx;
 681
 682         IFCQ_LOCK_ASSERT_HELD(tif->tif_ifq);
 683
 684         for (idx = tif->tif_maxpri; idx >= 0; idx--)
 685                 if ((cl = tif->tif_classes[idx]) != NULL &&
 686                     cl->cl_handle == chandle)
 687                         return (cl);
 688
 689         return (NULL);
 690 }
 691
 692 static const char *
 693 tcq_style(struct tcq_if *tif)
 694 {
 695 #pragma unused(tif)
 696         return ("TCQ");
 697 }
 698
 699 /*
 700  * tcq_enqueue_ifclassq is an enqueue function to be registered to
 701  * (*ifcq_enqueue) in struct ifclassq.
 702  */
 703 static int
 704 tcq_enqueue_ifclassq(struct ifclassq *ifq, void *p, classq_pkt_type_t ptype,
 705     boolean_t *pdrop)
 706 {
 707         u_int32_t i = 0;
 708         int ret;
 709         pktsched_pkt_t pkt;
 710         struct pf_mtag *t = NULL;
 711
 712         IFCQ_LOCK_ASSERT_HELD(ifq);
 713
 714         if (ptype == QP_MBUF) {
 715                 struct mbuf *m = p;
 716                 if (!(m->m_flags & M_PKTHDR)) {
 717                         /* should not happen */
 718                         log(LOG_ERR, "%s: packet does not have pkthdr\n",
 719                             if_name(ifq->ifcq_ifp));
 720                         IFCQ_CONVERT_LOCK(ifq);
 721                         m_freem(m);
 722                         *pdrop = TRUE;
 723                         return (ENOBUFS);
 724                 }
 725                 t =  m_pftag(m);
 726                 i = MBUF_SCIDX(mbuf_get_service_class(m));
 727         }
 728         VERIFY((u_int32_t)i < IFCQ_SC_MAX);
 729
 730         pktsched_pkt_encap(&pkt, ptype, p);
 731
 732         ret = tcq_enqueue(ifq->ifcq_disc,
 733             ifq->ifcq_disc_slots[i].cl, &pkt, t);
 734
 735         if ((ret != 0) && (ret != CLASSQEQ_SUCCESS_FC)) {
 736                 pktsched_free_pkt(&pkt);
 737                 *pdrop = TRUE;
 738         } else {
 739                 *pdrop = FALSE;
 740         }
 741
 742         switch (ret) {
 743         case CLASSQEQ_DROP:
 744                 ret = ENOBUFS;
 745                 break;
 746         case CLASSQEQ_DROP_FC:
 747                 ret = EQFULL;
 748                 break;
 749         case CLASSQEQ_DROP_SP:
 750                 ret = EQSUSPENDED;
 751                 break;
 752         case CLASSQEQ_SUCCESS_FC:
 753                 ret = EQFULL;
 754                 break;
 755         case CLASSQEQ_SUCCESS:
 756                 ret = 0;
 757                 break;
 758         default:
 759                 VERIFY(0);
 760         }
 761         return (ret);
 762 }
 763
 764 /*
 765  * tcq_dequeue_tc_ifclassq is a dequeue function to be registered to
 766  * (*ifcq_dequeue) in struct ifclass.
 767  *
 768  * note: CLASSQDQ_POLL returns the next packet without removing the packet
 769  *      from the queue.  CLASSQDQ_REMOVE is a normal dequeue operation.
 770  *      CLASSQDQ_REMOVE must return the same packet if called immediately
 771  *      after CLASSQDQ_POLL.
 772  */
 773 static void *
 774 tcq_dequeue_tc_ifclassq(struct ifclassq *ifq, mbuf_svc_class_t sc,
 775     classq_pkt_type_t *ptype)
 776 {
 777         pktsched_pkt_t pkt;
 778         u_int32_t i = MBUF_SCIDX(sc);
 779
 780         VERIFY((u_int32_t)i < IFCQ_SC_MAX);
 781
 782         bzero(&pkt, sizeof (pkt));
 783         (tcq_dequeue_cl(ifq->ifcq_disc, ifq->ifcq_disc_slots[i].cl, sc, &pkt));
 784         *ptype = pkt.pktsched_ptype;
 785         return (pkt.pktsched_pkt);
 786 }
 787
 788 static int
 789 tcq_request_ifclassq(struct ifclassq *ifq, cqrq_t req, void *arg)
 790 {
 791         struct tcq_if   *tif = (struct tcq_if *)ifq->ifcq_disc;
 792         int err = 0;
 793
 794         IFCQ_LOCK_ASSERT_HELD(ifq);
 795
 796         switch (req) {
 797         case CLASSQRQ_PURGE:
 798                 tcq_purge(tif);
 799                 break;
 800
 801         case CLASSQRQ_PURGE_SC:
 802                 tcq_purge_sc(tif, (cqrq_purge_sc_t *)arg);
 803                 break;
 804
 805         case CLASSQRQ_EVENT:
 806                 tcq_event(tif, (cqev_t)arg);
 807                 break;
 808
 809         case CLASSQRQ_THROTTLE:
 810                 err = tcq_throttle(tif, (cqrq_throttle_t *)arg);
 811                 break;
 812
 813         case CLASSQRQ_STAT_SC:
 814                 err = tcq_stat_sc(tif, (cqrq_stat_sc_t *)arg);
 815                 break;
 816         }
 817         return (err);
 818 }
 819
 820 int
 821 tcq_setup_ifclassq(struct ifclassq *ifq, u_int32_t flags,
 822     classq_pkt_type_t ptype)
 823 {
 824         struct ifnet *ifp = ifq->ifcq_ifp;
 825         struct tcq_class *cl0, *cl1, *cl2, *cl3;
 826         struct tcq_if *tif;
 827         u_int32_t maxlen = 0, qflags = 0;
 828         int err = 0;
 829
 830         IFCQ_LOCK_ASSERT_HELD(ifq);
 831         VERIFY(ifq->ifcq_disc == NULL);
 832         VERIFY(ifq->ifcq_type == PKTSCHEDT_NONE);
 833
 834         if (flags & PKTSCHEDF_QALG_SFB)
 835                 qflags |= TQCF_SFB;
 836         if (flags & PKTSCHEDF_QALG_ECN)
 837                 qflags |= TQCF_ECN;
 838         if (flags & PKTSCHEDF_QALG_FLOWCTL)
 839                 qflags |= TQCF_FLOWCTL;
 840         if (flags & PKTSCHEDF_QALG_DELAYBASED)
 841                 qflags |= TQCF_DELAYBASED;
 842
 843         tif = tcq_alloc(ifp, M_WAITOK);
 844         if (tif == NULL)
 845                 return (ENOMEM);
 846
 847         if ((maxlen = IFCQ_MAXLEN(ifq)) == 0)
 848                 maxlen = if_sndq_maxlen;
 849
 850         if ((err = tcq_add_queue(tif, 0, maxlen,
 851             qflags | TQCF_LAZY, SCIDX_BK, &cl0, ptype)) != 0)
 852                 goto cleanup;
 853
 854         if ((err = tcq_add_queue(tif, 1, maxlen,
 855             qflags | TQCF_DEFAULTCLASS, SCIDX_BE, &cl1, ptype)) != 0)
 856                 goto cleanup;
 857
 858         if ((err = tcq_add_queue(tif, 2, maxlen,
 859             qflags | TQCF_LAZY, SCIDX_VI, &cl2, ptype)) != 0)
 860                 goto cleanup;
 861
 862         if ((err = tcq_add_queue(tif, 3, maxlen,
 863             qflags, SCIDX_VO, &cl3, ptype)) != 0)
 864                 goto cleanup;
 865
 866         err = ifclassq_attach(ifq, PKTSCHEDT_TCQ, tif,
 867             tcq_enqueue_ifclassq, NULL, tcq_dequeue_tc_ifclassq,
 868             NULL, NULL, tcq_request_ifclassq);
 869
 870         /* cache these for faster lookup */
 871         if (err == 0) {
 872                 /* Map {BK_SYS,BK} to TC_BK */
 873                 ifq->ifcq_disc_slots[SCIDX_BK_SYS].qid = SCIDX_BK;
 874                 ifq->ifcq_disc_slots[SCIDX_BK_SYS].cl = cl0;
 875
 876                 ifq->ifcq_disc_slots[SCIDX_BK].qid = SCIDX_BK;
 877                 ifq->ifcq_disc_slots[SCIDX_BK].cl = cl0;
 878
 879                 /* Map {BE,RD,OAM} to TC_BE */
 880                 ifq->ifcq_disc_slots[SCIDX_BE].qid = SCIDX_BE;
 881                 ifq->ifcq_disc_slots[SCIDX_BE].cl = cl1;
 882
 883                 ifq->ifcq_disc_slots[SCIDX_RD].qid = SCIDX_BE;
 884                 ifq->ifcq_disc_slots[SCIDX_RD].cl = cl1;
 885
 886                 ifq->ifcq_disc_slots[SCIDX_OAM].qid = SCIDX_BE;
 887                 ifq->ifcq_disc_slots[SCIDX_OAM].cl = cl1;
 888
 889                 /* Map {AV,RV,VI} to TC_VI */
 890                 ifq->ifcq_disc_slots[SCIDX_AV].qid = SCIDX_VI;
 891                 ifq->ifcq_disc_slots[SCIDX_AV].cl = cl2;
 892
 893                 ifq->ifcq_disc_slots[SCIDX_RV].qid = SCIDX_VI;
 894                 ifq->ifcq_disc_slots[SCIDX_RV].cl = cl2;
 895
 896                 ifq->ifcq_disc_slots[SCIDX_VI].qid = SCIDX_VI;
 897                 ifq->ifcq_disc_slots[SCIDX_VI].cl = cl2;
 898
 899                 /* Map {VO,CTL} to TC_VO */
 900                 ifq->ifcq_disc_slots[SCIDX_VO].qid = SCIDX_VO;
 901                 ifq->ifcq_disc_slots[SCIDX_VO].cl = cl3;
 902
 903                 ifq->ifcq_disc_slots[SCIDX_CTL].qid = SCIDX_VO;
 904                 ifq->ifcq_disc_slots[SCIDX_CTL].cl = cl3;
 905         }
 906
 907 cleanup:
 908         if (err != 0)
 909                 (void) tcq_destroy_locked(tif);
 910
 911         return (err);
 912 }
 913
 914 int
 915 tcq_teardown_ifclassq(struct ifclassq *ifq)
 916 {
 917         struct tcq_if *tif = ifq->ifcq_disc;
 918         int i;
 919
 920         IFCQ_LOCK_ASSERT_HELD(ifq);
 921         VERIFY(tif != NULL && ifq->ifcq_type == PKTSCHEDT_TCQ);
 922
 923         (void) tcq_destroy_locked(tif);
 924
 925         ifq->ifcq_disc = NULL;
 926         for (i = 0; i < IFCQ_SC_MAX; i++) {
 927                 ifq->ifcq_disc_slots[i].qid = 0;
 928                 ifq->ifcq_disc_slots[i].cl = NULL;
 929         }
 930
 931         return (ifclassq_detach(ifq));
 932 }
 933
 934 int
 935 tcq_getqstats_ifclassq(struct ifclassq *ifq, u_int32_t slot,
 936     struct if_ifclassq_stats *ifqs)
 937 {
 938         struct tcq_if *tif = ifq->ifcq_disc;
 939
 940         IFCQ_LOCK_ASSERT_HELD(ifq);
 941         VERIFY(ifq->ifcq_type == PKTSCHEDT_TCQ);
 942
 943         if (slot >= IFCQ_SC_MAX)
 944                 return (EINVAL);
 945
 946         return (tcq_get_class_stats(tif, ifq->ifcq_disc_slots[slot].qid,
 947             &ifqs->ifqs_tcq_stats));
 948 }
 949
 950 static int
 951 tcq_throttle(struct tcq_if *tif, cqrq_throttle_t *tr)
 952 {
 953         struct ifclassq *ifq = tif->tif_ifq;
 954         struct tcq_class *cl;
 955         int err = 0;
 956
 957         IFCQ_LOCK_ASSERT_HELD(ifq);
 958
 959         if (!tr->set) {
 960                 tr->level = tif->tif_throttle;
 961                 return (0);
 962         }
 963
 964         if (tr->level == tif->tif_throttle)
 965                 return (EALREADY);
 966
 967         /* Current throttling levels only involve BK_SYS class */
 968         cl = ifq->ifcq_disc_slots[SCIDX_BK_SYS].cl;
 969
 970         switch (tr->level) {
 971         case IFNET_THROTTLE_OFF:
 972                 err = tcq_resumeq(tif, cl);
 973                 break;
 974
 975         case IFNET_THROTTLE_OPPORTUNISTIC:
 976                 err = tcq_suspendq(tif, cl);
 977                 break;
 978
 979         default:
 980                 VERIFY(0);
 981                 /* NOTREACHED */
 982         }
 983
 984         if (err == 0 || err == ENXIO) {
 985                 if (pktsched_verbose) {
 986                         log(LOG_DEBUG, "%s: %s throttling %slevel set %d->%d\n",
 987                             if_name(TCQIF_IFP(tif)), tcq_style(tif),
 988                             (err == 0) ? "" : "lazy ", tif->tif_throttle,
 989                             tr->level);
 990                 }
 991                 tif->tif_throttle = tr->level;
 992                 if (err != 0)
 993                         err = 0;
 994                 else
 995                         tcq_purgeq(tif, cl, 0, NULL, NULL);
 996         } else {
 997                 log(LOG_ERR, "%s: %s unable to set throttling level "
 998                     "%d->%d [error=%d]\n", if_name(TCQIF_IFP(tif)),
 999                     tcq_style(tif), tif->tif_throttle, tr->level, err);
1000         }
1001
1002         return (err);
1003 }
1004
1005 static int
1006 tcq_resumeq(struct tcq_if *tif, struct tcq_class *cl)
1007 {
1008         struct ifclassq *ifq = tif->tif_ifq;
1009         int err = 0;
1010 #if !MACH_ASSERT
1011 #pragma unused(ifq)
1012 #endif
1013         IFCQ_LOCK_ASSERT_HELD(ifq);
1014
1015         if (q_is_sfb(&cl->cl_q) && cl->cl_sfb != NULL)
1016                 err = sfb_suspendq(cl->cl_sfb, &cl->cl_q, FALSE);
1017
1018         if (err == 0)
1019                 qstate(&cl->cl_q) = QS_RUNNING;
1020
1021         return (err);
1022 }
1023
1024 static int
1025 tcq_suspendq(struct tcq_if *tif, struct tcq_class *cl)
1026 {
1027         struct ifclassq *ifq = tif->tif_ifq;
1028         int err = 0;
1029 #if !MACH_ASSERT
1030 #pragma unused(ifq)
1031 #endif
1032         IFCQ_LOCK_ASSERT_HELD(ifq);
1033
1034         if (q_is_sfb(&cl->cl_q)) {
1035                 if (cl->cl_sfb != NULL) {
1036                         err = sfb_suspendq(cl->cl_sfb, &cl->cl_q, TRUE);
1037                 } else {
1038                         VERIFY(cl->cl_flags & TQCF_LAZY);
1039                         err = ENXIO;    /* delayed throttling */
1040                 }
1041         }
1042
1043         if (err == 0 || err == ENXIO)
1044                 qstate(&cl->cl_q) = QS_SUSPENDED;
1045
1046         return (err);
1047 }