bsd/net/classq/classq_subr.c

   1 /*
   2  * Copyright (c) 2011-2015 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 #include <sys/cdefs.h>
  30 #include <sys/param.h>
  31 #include <sys/mbuf.h>
  32 #include <sys/errno.h>
  33 #include <sys/random.h>
  34 #include <sys/kernel_types.h>
  35 #include <sys/sysctl.h>
  36
  37 #include <kern/zalloc.h>
  38
  39 #include <net/if.h>
  40 #include <net/net_osdep.h>
  41 #include <net/classq/classq.h>
  42 #if CLASSQ_RED
  43 #include <net/classq/classq_red.h>
  44 #endif /* CLASSQ_RED */
  45 #if CLASSQ_RIO
  46 #include <net/classq/classq_rio.h>
  47 #endif /* CLASSQ_RIO */
  48 #if CLASSQ_BLUE
  49 #include <net/classq/classq_blue.h>
  50 #endif /* CLASSQ_BLUE */
  51 #include <net/classq/classq_sfb.h>
  52 #include <net/pktsched/pktsched.h>
  53
  54 #include <libkern/libkern.h>
  55
  56 #if PF_ALTQ
  57 #include <net/altq/altq.h>
  58 #endif /* PF_ALTQ */
  59
  60 static errno_t ifclassq_dequeue_common(struct ifclassq *, mbuf_svc_class_t,
  61     u_int32_t, struct mbuf **, struct mbuf **, u_int32_t *, u_int32_t *,
  62     boolean_t);
  63 static struct mbuf *ifclassq_poll_common(struct ifclassq *,
  64     mbuf_svc_class_t, boolean_t);
  65 static struct mbuf *ifclassq_tbr_dequeue_common(struct ifclassq *, int,
  66     mbuf_svc_class_t, boolean_t);
  67
  68 void
  69 classq_init(void)
  70 {
  71         _CASSERT(MBUF_TC_BE == 0);
  72         _CASSERT(MBUF_SC_BE == 0);
  73         _CASSERT(IFCQ_SC_MAX == MBUF_SC_MAX_CLASSES);
  74
  75 #if CLASSQ_RED
  76         red_init();
  77 #endif /* CLASSQ_RED */
  78 #if CLASSQ_RIO
  79         rio_init();
  80 #endif /* CLASSQ_RIO */
  81 #if CLASSQ_BLUE
  82         blue_init();
  83 #endif /* CLASSQ_BLUE */
  84         sfb_init();
  85 }
  86
  87 int
  88 ifclassq_setup(struct ifnet *ifp, u_int32_t sflags, boolean_t reuse)
  89 {
  90 #pragma unused(reuse)
  91         struct ifclassq *ifq = &ifp->if_snd;
  92         int err = 0;
  93
  94         IFCQ_LOCK(ifq);
  95         VERIFY(IFCQ_IS_EMPTY(ifq));
  96         ifq->ifcq_ifp = ifp;
  97         IFCQ_LEN(ifq) = 0;
  98         IFCQ_BYTES(ifq) = 0;
  99         bzero(&ifq->ifcq_xmitcnt, sizeof (ifq->ifcq_xmitcnt));
 100         bzero(&ifq->ifcq_dropcnt, sizeof (ifq->ifcq_dropcnt));
 101
 102         VERIFY(!IFCQ_TBR_IS_ENABLED(ifq));
 103         VERIFY(ifq->ifcq_type == PKTSCHEDT_NONE);
 104         VERIFY(ifq->ifcq_flags == 0);
 105         VERIFY(ifq->ifcq_sflags == 0);
 106         VERIFY(ifq->ifcq_disc == NULL);
 107         VERIFY(ifq->ifcq_enqueue == NULL);
 108         VERIFY(ifq->ifcq_dequeue == NULL);
 109         VERIFY(ifq->ifcq_dequeue_sc == NULL);
 110         VERIFY(ifq->ifcq_request == NULL);
 111
 112         if (ifp->if_eflags & IFEF_TXSTART) {
 113                 u_int32_t maxlen = 0;
 114
 115                 if ((maxlen = IFCQ_MAXLEN(ifq)) == 0)
 116                         maxlen = if_sndq_maxlen;
 117                 IFCQ_SET_MAXLEN(ifq, maxlen);
 118
 119                 if (IFCQ_MAXLEN(ifq) != if_sndq_maxlen &&
 120                     IFCQ_TARGET_QDELAY(ifq) == 0) {
 121                         /*
 122                          * Choose static queues because the interface has
 123                          * maximum queue size set
 124                          */
 125                         sflags &= ~PKTSCHEDF_QALG_DELAYBASED;
 126                 }
 127                 ifq->ifcq_sflags = sflags;
 128                 err = ifclassq_pktsched_setup(ifq);
 129                 if (err == 0)
 130                         ifq->ifcq_flags = (IFCQF_READY | IFCQF_ENABLED);
 131         }
 132
 133 #if PF_ALTQ
 134         ifq->ifcq_drain = 0;
 135         IFCQ_ALTQ(ifq)->altq_ifcq = ifq;
 136         VERIFY(IFCQ_ALTQ(ifq)->altq_type == ALTQT_NONE);
 137         VERIFY(IFCQ_ALTQ(ifq)->altq_flags == 0);
 138         VERIFY(IFCQ_ALTQ(ifq)->altq_disc == NULL);
 139         VERIFY(IFCQ_ALTQ(ifq)->altq_enqueue == NULL);
 140         VERIFY(IFCQ_ALTQ(ifq)->altq_dequeue == NULL);
 141         VERIFY(IFCQ_ALTQ(ifq)->altq_dequeue_sc == NULL);
 142         VERIFY(IFCQ_ALTQ(ifq)->altq_request == NULL);
 143
 144         if ((ifp->if_eflags & IFEF_TXSTART) &&
 145             ifp->if_output_sched_model != IFNET_SCHED_MODEL_DRIVER_MANAGED)
 146                 ALTQ_SET_READY(IFCQ_ALTQ(ifq));
 147         else
 148                 ALTQ_CLEAR_READY(IFCQ_ALTQ(ifq));
 149 #endif /* PF_ALTQ */
 150         IFCQ_UNLOCK(ifq);
 151
 152         return (err);
 153 }
 154
 155 void
 156 ifclassq_teardown(struct ifnet *ifp)
 157 {
 158         struct ifclassq *ifq = &ifp->if_snd;
 159
 160         IFCQ_LOCK(ifq);
 161 #if PF_ALTQ
 162         if (ALTQ_IS_READY(IFCQ_ALTQ(ifq))) {
 163                 if (ALTQ_IS_ENABLED(IFCQ_ALTQ(ifq)))
 164                         altq_disable(IFCQ_ALTQ(ifq));
 165                 if (ALTQ_IS_ATTACHED(IFCQ_ALTQ(ifq)))
 166                         altq_detach(IFCQ_ALTQ(ifq));
 167                 IFCQ_ALTQ(ifq)->altq_flags = 0;
 168         }
 169         ifq->ifcq_drain = 0;
 170         IFCQ_ALTQ(ifq)->altq_ifcq = NULL;
 171         VERIFY(IFCQ_ALTQ(ifq)->altq_type == ALTQT_NONE);
 172         VERIFY(IFCQ_ALTQ(ifq)->altq_flags == 0);
 173         VERIFY(IFCQ_ALTQ(ifq)->altq_disc == NULL);
 174         VERIFY(IFCQ_ALTQ(ifq)->altq_enqueue == NULL);
 175         VERIFY(IFCQ_ALTQ(ifq)->altq_dequeue == NULL);
 176         VERIFY(IFCQ_ALTQ(ifq)->altq_dequeue_sc == NULL);
 177         VERIFY(IFCQ_ALTQ(ifq)->altq_request == NULL);
 178 #endif /* PF_ALTQ */
 179
 180         if (IFCQ_IS_READY(ifq)) {
 181                 if (IFCQ_TBR_IS_ENABLED(ifq)) {
 182                         struct tb_profile tb = { 0, 0, 0 };
 183                         (void) ifclassq_tbr_set(ifq, &tb, FALSE);
 184                 }
 185                 (void) pktsched_teardown(ifq);
 186                 ifq->ifcq_flags = 0;
 187         }
 188         ifq->ifcq_sflags = 0;
 189
 190         VERIFY(IFCQ_IS_EMPTY(ifq));
 191         VERIFY(!IFCQ_TBR_IS_ENABLED(ifq));
 192         VERIFY(ifq->ifcq_type == PKTSCHEDT_NONE);
 193         VERIFY(ifq->ifcq_flags == 0);
 194         VERIFY(ifq->ifcq_sflags == 0);
 195         VERIFY(ifq->ifcq_disc == NULL);
 196         VERIFY(ifq->ifcq_enqueue == NULL);
 197         VERIFY(ifq->ifcq_dequeue == NULL);
 198         VERIFY(ifq->ifcq_dequeue_sc == NULL);
 199         VERIFY(ifq->ifcq_request == NULL);
 200         IFCQ_LEN(ifq) = 0;
 201         IFCQ_BYTES(ifq) = 0;
 202         IFCQ_MAXLEN(ifq) = 0;
 203         bzero(&ifq->ifcq_xmitcnt, sizeof (ifq->ifcq_xmitcnt));
 204         bzero(&ifq->ifcq_dropcnt, sizeof (ifq->ifcq_dropcnt));
 205
 206         IFCQ_UNLOCK(ifq);
 207 }
 208
 209 int
 210 ifclassq_pktsched_setup(struct ifclassq *ifq)
 211 {
 212         struct ifnet *ifp = ifq->ifcq_ifp;
 213         int err = 0;
 214
 215         IFCQ_LOCK_ASSERT_HELD(ifq);
 216         VERIFY(ifp->if_eflags & IFEF_TXSTART);
 217
 218         switch (ifp->if_output_sched_model) {
 219         case IFNET_SCHED_MODEL_DRIVER_MANAGED:
 220                 err = pktsched_setup(ifq, PKTSCHEDT_TCQ, ifq->ifcq_sflags);
 221                 break;
 222
 223         case IFNET_SCHED_MODEL_NORMAL:
 224                 err = pktsched_setup(ifq, PKTSCHEDT_QFQ, ifq->ifcq_sflags);
 225                 break;
 226
 227         default:
 228                 VERIFY(0);
 229                 /* NOTREACHED */
 230         }
 231
 232         return (err);
 233 }
 234
 235 void
 236 ifclassq_set_maxlen(struct ifclassq *ifq, u_int32_t maxqlen)
 237 {
 238         IFCQ_LOCK(ifq);
 239         if (maxqlen == 0)
 240                 maxqlen = if_sndq_maxlen;
 241         IFCQ_SET_MAXLEN(ifq, maxqlen);
 242         IFCQ_UNLOCK(ifq);
 243 }
 244
 245 u_int32_t
 246 ifclassq_get_maxlen(struct ifclassq *ifq)
 247 {
 248         return (IFCQ_MAXLEN(ifq));
 249 }
 250
 251 int
 252 ifclassq_get_len(struct ifclassq *ifq, mbuf_svc_class_t sc, u_int32_t *packets,
 253     u_int32_t *bytes)
 254 {
 255         int err = 0;
 256
 257         IFCQ_LOCK(ifq);
 258         if (sc == MBUF_SC_UNSPEC) {
 259                 VERIFY(packets != NULL);
 260                 *packets = IFCQ_LEN(ifq);
 261         } else {
 262                 VERIFY(MBUF_VALID_SC(sc));
 263                 VERIFY(packets != NULL && bytes != NULL);
 264                 IFCQ_LEN_SC(ifq, sc, packets, bytes, err);
 265         }
 266         IFCQ_UNLOCK(ifq);
 267
 268         return (err);
 269 }
 270
 271 errno_t
 272 ifclassq_enqueue(struct ifclassq *ifq, struct mbuf *m)
 273 {
 274         errno_t err;
 275
 276         IFCQ_LOCK_SPIN(ifq);
 277
 278 #if PF_ALTQ
 279         if (ALTQ_IS_ENABLED(IFCQ_ALTQ(ifq))) {
 280                 ALTQ_ENQUEUE(IFCQ_ALTQ(ifq), m, err);
 281         } else {
 282                 u_int32_t qlen = IFCQ_LEN(ifq);
 283                 IFCQ_ENQUEUE(ifq, m, err);
 284                 if (IFCQ_LEN(ifq) > qlen)
 285                         ifq->ifcq_drain += (IFCQ_LEN(ifq) - qlen);
 286         }
 287 #else /* !PF_ALTQ */
 288         IFCQ_ENQUEUE(ifq, m, err);
 289 #endif /* PF_ALTQ */
 290
 291         IFCQ_UNLOCK(ifq);
 292
 293         return (err);
 294 }
 295
 296 errno_t
 297 ifclassq_dequeue(struct ifclassq *ifq, u_int32_t limit, struct mbuf **head,
 298     struct mbuf **tail, u_int32_t *cnt, u_int32_t *len)
 299 {
 300         return (ifclassq_dequeue_common(ifq, MBUF_SC_UNSPEC, limit, head, tail,
 301             cnt, len, FALSE));
 302 }
 303
 304 errno_t
 305 ifclassq_dequeue_sc(struct ifclassq *ifq, mbuf_svc_class_t sc,
 306     u_int32_t limit, struct mbuf **head, struct mbuf **tail, u_int32_t *cnt,
 307     u_int32_t *len)
 308 {
 309         return (ifclassq_dequeue_common(ifq, sc, limit, head, tail,
 310             cnt, len, TRUE));
 311 }
 312
 313 static errno_t
 314 ifclassq_dequeue_common(struct ifclassq *ifq, mbuf_svc_class_t sc,
 315     u_int32_t limit, struct mbuf **head, struct mbuf **tail, u_int32_t *cnt,
 316     u_int32_t *len, boolean_t drvmgt)
 317 {
 318         struct ifnet *ifp = ifq->ifcq_ifp;
 319         u_int32_t i = 0, l = 0;
 320         struct mbuf **first, *last;
 321 #if PF_ALTQ
 322         struct ifaltq *altq = IFCQ_ALTQ(ifq);
 323         boolean_t draining;
 324 #endif /* PF_ALTQ */
 325
 326         VERIFY(!drvmgt || MBUF_VALID_SC(sc));
 327
 328         *head = NULL;
 329         first = &(*head);
 330         last = NULL;
 331
 332         ifq = &ifp->if_snd;
 333         IFCQ_LOCK_SPIN(ifq);
 334
 335         while (i < limit) {
 336 #if PF_ALTQ
 337                 u_int32_t qlen;
 338
 339                 qlen = IFCQ_LEN(ifq);
 340                 draining = IFCQ_IS_DRAINING(ifq);
 341
 342                 if (drvmgt) {
 343                         if (IFCQ_TBR_IS_ENABLED(ifq))
 344                                 IFCQ_TBR_DEQUEUE_SC(ifq, sc, *head);
 345                         else if (draining)
 346                                 IFCQ_DEQUEUE_SC(ifq, sc, *head);
 347                         else if (ALTQ_IS_ENABLED(altq))
 348                                 ALTQ_DEQUEUE_SC(altq, sc, *head);
 349                         else
 350                                 *head = NULL;
 351                 } else {
 352                         if (IFCQ_TBR_IS_ENABLED(ifq))
 353                                 IFCQ_TBR_DEQUEUE(ifq, *head);
 354                         else if (draining)
 355                                 IFCQ_DEQUEUE(ifq, *head);
 356                         else if (ALTQ_IS_ENABLED(altq))
 357                                 ALTQ_DEQUEUE(altq, *head);
 358                         else
 359                                 *head = NULL;
 360                 }
 361
 362                 if (draining && *head != NULL) {
 363                         VERIFY(ifq->ifcq_drain >= (qlen - IFCQ_LEN(ifq)));
 364                         ifq->ifcq_drain -= (qlen - IFCQ_LEN(ifq));
 365                 }
 366 #else /* ! PF_ALTQ */
 367                 if (drvmgt) {
 368                         if (IFCQ_TBR_IS_ENABLED(ifq))
 369                                 IFCQ_TBR_DEQUEUE_SC(ifq, sc, *head);
 370                         else
 371                                 IFCQ_DEQUEUE_SC(ifq, sc, *head);
 372                 } else {
 373                         if (IFCQ_TBR_IS_ENABLED(ifq))
 374                                 IFCQ_TBR_DEQUEUE(ifq, *head);
 375                         else
 376                                 IFCQ_DEQUEUE(ifq, *head);
 377                 }
 378 #endif /* !PF_ALTQ */
 379
 380                 if (*head == NULL)
 381                         break;
 382
 383                 (*head)->m_nextpkt = NULL;
 384                 last = *head;
 385
 386                 l += (*head)->m_pkthdr.len;
 387
 388 #if MEASURE_BW
 389                 (*head)->m_pkthdr.pkt_bwseq =
 390                     atomic_add_64_ov(&(ifp->if_bw.cur_seq), m_pktlen(*head));
 391 #endif /* MEASURE_BW */
 392                 if (IFNET_IS_CELLULAR(ifp)) {
 393                         (*head)->m_pkthdr.pkt_flags |= PKTF_VALID_UNSENT_DATA;
 394                         (*head)->m_pkthdr.pkt_unsent_databytes =
 395                             (total_snd_byte_count << MSIZESHIFT) +
 396                             ifq->ifcq_bytes;
 397                 }
 398                 head = &(*head)->m_nextpkt;
 399                 i++;
 400         }
 401
 402         IFCQ_UNLOCK(ifq);
 403
 404         if (tail != NULL)
 405                 *tail = last;
 406         if (cnt != NULL)
 407                 *cnt = i;
 408         if (len != NULL)
 409                 *len = l;
 410
 411         return ((*first != NULL) ? 0 : EAGAIN);
 412 }
 413
 414 struct mbuf *
 415 ifclassq_poll(struct ifclassq *ifq)
 416 {
 417         return (ifclassq_poll_common(ifq, MBUF_SC_UNSPEC, FALSE));
 418 }
 419
 420 struct mbuf *
 421 ifclassq_poll_sc(struct ifclassq *ifq, mbuf_svc_class_t sc)
 422 {
 423         return (ifclassq_poll_common(ifq, sc, TRUE));
 424 }
 425
 426 static struct mbuf *
 427 ifclassq_poll_common(struct ifclassq *ifq, mbuf_svc_class_t sc,
 428     boolean_t drvmgt)
 429 {
 430 #if PF_ALTQ
 431         struct ifaltq *altq = IFCQ_ALTQ(ifq);
 432 #endif /* PF_ALTQ */
 433         struct mbuf *m;
 434
 435         VERIFY(!drvmgt || MBUF_VALID_SC(sc));
 436
 437 #if PF_ALTQ
 438         if (drvmgt) {
 439                 if (IFCQ_TBR_IS_ENABLED(ifq))
 440                         IFCQ_TBR_POLL_SC(ifq, sc, m);
 441                 else if (IFCQ_IS_DRAINING(ifq))
 442                         IFCQ_POLL_SC(ifq, sc, m);
 443                 else if (ALTQ_IS_ENABLED(altq))
 444                         ALTQ_POLL_SC(altq, sc, m);
 445                 else
 446                         m = NULL;
 447         } else {
 448                 if (IFCQ_TBR_IS_ENABLED(ifq))
 449                         IFCQ_TBR_POLL(ifq, m);
 450                 else if (IFCQ_IS_DRAINING(ifq))
 451                         IFCQ_POLL(ifq, m);
 452                 else if (ALTQ_IS_ENABLED(altq))
 453                         ALTQ_POLL(altq, m);
 454                 else
 455                         m = NULL;
 456         }
 457 #else /* ! PF_ALTQ */
 458         if (drvmgt) {
 459                 if (IFCQ_TBR_IS_ENABLED(ifq))
 460                         IFCQ_TBR_POLL_SC(ifq, sc, m);
 461                 else
 462                         IFCQ_POLL_SC(ifq, sc, m);
 463         } else {
 464                 if (IFCQ_TBR_IS_ENABLED(ifq))
 465                         IFCQ_TBR_POLL(ifq, m);
 466                 else
 467                         IFCQ_POLL(ifq, m);
 468         }
 469 #endif /* !PF_ALTQ */
 470
 471         return (m);
 472 }
 473
 474 void
 475 ifclassq_update(struct ifclassq *ifq, cqev_t ev)
 476 {
 477         IFCQ_LOCK_ASSERT_HELD(ifq);
 478         VERIFY(IFCQ_IS_READY(ifq));
 479
 480 #if PF_ALTQ
 481         if (ALTQ_IS_ENABLED(IFCQ_ALTQ(ifq)))
 482                 ALTQ_UPDATE(IFCQ_ALTQ(ifq), ev);
 483 #endif /* PF_ALTQ */
 484         IFCQ_UPDATE(ifq, ev);
 485 }
 486
 487 int
 488 ifclassq_attach(struct ifclassq *ifq, u_int32_t type, void *discipline,
 489     ifclassq_enq_func enqueue, ifclassq_deq_func dequeue,
 490     ifclassq_deq_sc_func dequeue_sc, ifclassq_req_func request)
 491 {
 492         IFCQ_LOCK_ASSERT_HELD(ifq);
 493
 494         VERIFY(ifq->ifcq_disc == NULL);
 495         VERIFY(enqueue != NULL);
 496         VERIFY(!(dequeue != NULL && dequeue_sc != NULL));
 497         VERIFY(request != NULL);
 498
 499         ifq->ifcq_type = type;
 500         ifq->ifcq_disc = discipline;
 501         ifq->ifcq_enqueue = enqueue;
 502         ifq->ifcq_dequeue = dequeue;
 503         ifq->ifcq_dequeue_sc = dequeue_sc;
 504         ifq->ifcq_request = request;
 505
 506         return (0);
 507 }
 508
 509 int
 510 ifclassq_detach(struct ifclassq *ifq)
 511 {
 512         IFCQ_LOCK_ASSERT_HELD(ifq);
 513
 514         VERIFY(ifq->ifcq_disc == NULL);
 515
 516         ifq->ifcq_type = PKTSCHEDT_NONE;
 517         ifq->ifcq_disc = NULL;
 518         ifq->ifcq_enqueue = NULL;
 519         ifq->ifcq_dequeue = NULL;
 520         ifq->ifcq_dequeue_sc = NULL;
 521         ifq->ifcq_request = NULL;
 522
 523         return (0);
 524 }
 525
 526 int
 527 ifclassq_getqstats(struct ifclassq *ifq, u_int32_t qid, void *ubuf,
 528     u_int32_t *nbytes)
 529 {
 530         struct if_ifclassq_stats *ifqs;
 531         int err;
 532
 533         if (*nbytes < sizeof (*ifqs))
 534                 return (EINVAL);
 535
 536         ifqs = _MALLOC(sizeof (*ifqs), M_TEMP, M_WAITOK | M_ZERO);
 537         if (ifqs == NULL)
 538                 return (ENOMEM);
 539
 540         IFCQ_LOCK(ifq);
 541         if (!IFCQ_IS_READY(ifq)) {
 542                 IFCQ_UNLOCK(ifq);
 543                 _FREE(ifqs, M_TEMP);
 544                 return (ENXIO);
 545         }
 546
 547         ifqs->ifqs_len = IFCQ_LEN(ifq);
 548         ifqs->ifqs_maxlen = IFCQ_MAXLEN(ifq);
 549         *(&ifqs->ifqs_xmitcnt) = *(&ifq->ifcq_xmitcnt);
 550         *(&ifqs->ifqs_dropcnt) = *(&ifq->ifcq_dropcnt);
 551         ifqs->ifqs_scheduler = ifq->ifcq_type;
 552
 553         err = pktsched_getqstats(ifq, qid, ifqs);
 554         IFCQ_UNLOCK(ifq);
 555
 556         if (err == 0 && (err = copyout((caddr_t)ifqs,
 557             (user_addr_t)(uintptr_t)ubuf, sizeof (*ifqs))) == 0)
 558                 *nbytes = sizeof (*ifqs);
 559
 560         _FREE(ifqs, M_TEMP);
 561
 562         return (err);
 563 }
 564
 565 const char *
 566 ifclassq_ev2str(cqev_t ev)
 567 {
 568         const char *c;
 569
 570         switch (ev) {
 571         case CLASSQ_EV_LINK_BANDWIDTH:
 572                 c = "LINK_BANDWIDTH";
 573                 break;
 574
 575         case CLASSQ_EV_LINK_LATENCY:
 576                 c = "LINK_LATENCY";
 577                 break;
 578
 579         case CLASSQ_EV_LINK_MTU:
 580                 c = "LINK_MTU";
 581                 break;
 582
 583         case CLASSQ_EV_LINK_UP:
 584                 c = "LINK_UP";
 585                 break;
 586
 587         case CLASSQ_EV_LINK_DOWN:
 588                 c = "LINK_DOWN";
 589                 break;
 590
 591         default:
 592                 c = "UNKNOWN";
 593                 break;
 594         }
 595
 596         return (c);
 597 }
 598
 599 /*
 600  * internal representation of token bucket parameters
 601  *      rate:   byte_per_unittime << 32
 602  *              (((bits_per_sec) / 8) << 32) / machclk_freq
 603  *      depth:  byte << 32
 604  *
 605  */
 606 #define TBR_SHIFT       32
 607 #define TBR_SCALE(x)    ((int64_t)(x) << TBR_SHIFT)
 608 #define TBR_UNSCALE(x)  ((x) >> TBR_SHIFT)
 609
 610 struct mbuf *
 611 ifclassq_tbr_dequeue(struct ifclassq *ifq, int op)
 612 {
 613         return (ifclassq_tbr_dequeue_common(ifq, op, MBUF_SC_UNSPEC, FALSE));
 614 }
 615
 616 struct mbuf *
 617 ifclassq_tbr_dequeue_sc(struct ifclassq *ifq, int op, mbuf_svc_class_t sc)
 618 {
 619         return (ifclassq_tbr_dequeue_common(ifq, op, sc, TRUE));
 620 }
 621
 622 static struct mbuf *
 623 ifclassq_tbr_dequeue_common(struct ifclassq *ifq, int op,
 624     mbuf_svc_class_t sc, boolean_t drvmgt)
 625 {
 626         struct tb_regulator *tbr;
 627         struct mbuf *m;
 628         int64_t interval;
 629         u_int64_t now;
 630
 631         IFCQ_LOCK_ASSERT_HELD(ifq);
 632
 633         VERIFY(!drvmgt || MBUF_VALID_SC(sc));
 634         VERIFY(IFCQ_TBR_IS_ENABLED(ifq));
 635
 636         tbr = &ifq->ifcq_tbr;
 637         if (op == CLASSQDQ_REMOVE && tbr->tbr_lastop == CLASSQDQ_POLL) {
 638                 /* if this is a remove after poll, bypass tbr check */
 639         } else {
 640                 /* update token only when it is negative */
 641                 if (tbr->tbr_token <= 0) {
 642                         now = read_machclk();
 643                         interval = now - tbr->tbr_last;
 644                         if (interval >= tbr->tbr_filluptime) {
 645                                 tbr->tbr_token = tbr->tbr_depth;
 646                         } else {
 647                                 tbr->tbr_token += interval * tbr->tbr_rate;
 648                                 if (tbr->tbr_token > tbr->tbr_depth)
 649                                         tbr->tbr_token = tbr->tbr_depth;
 650                         }
 651                         tbr->tbr_last = now;
 652                 }
 653                 /* if token is still negative, don't allow dequeue */
 654                 if (tbr->tbr_token <= 0)
 655                         return (NULL);
 656         }
 657
 658         /*
 659          * ifclassq takes precedence over ALTQ queue;
 660          * ifcq_drain count is adjusted by the caller.
 661          */
 662 #if PF_ALTQ
 663         if (IFCQ_IS_DRAINING(ifq)) {
 664 #endif /* PF_ALTQ */
 665                 if (op == CLASSQDQ_POLL) {
 666                         if (drvmgt)
 667                                 IFCQ_POLL_SC(ifq, sc, m);
 668                         else
 669                                 IFCQ_POLL(ifq, m);
 670                 } else {
 671                         if (drvmgt)
 672                                 IFCQ_DEQUEUE_SC(ifq, sc, m);
 673                         else
 674                                 IFCQ_DEQUEUE(ifq, m);
 675                 }
 676 #if PF_ALTQ
 677         } else {
 678                 struct ifaltq *altq = IFCQ_ALTQ(ifq);
 679                 if (ALTQ_IS_ENABLED(altq)) {
 680                         if (drvmgt)
 681                                 m = (*altq->altq_dequeue_sc)(altq, sc, op);
 682                         else
 683                                 m = (*altq->altq_dequeue)(altq, op);
 684                 } else {
 685                         m = NULL;
 686                 }
 687         }
 688 #endif /* PF_ALTQ */
 689
 690         if (m != NULL && op == CLASSQDQ_REMOVE)
 691                 tbr->tbr_token -= TBR_SCALE(m_pktlen(m));
 692         tbr->tbr_lastop = op;
 693
 694         return (m);
 695 }
 696
 697 /*
 698  * set a token bucket regulator.
 699  * if the specified rate is zero, the token bucket regulator is deleted.
 700  */
 701 int
 702 ifclassq_tbr_set(struct ifclassq *ifq, struct tb_profile *profile,
 703     boolean_t update)
 704 {
 705         struct tb_regulator *tbr;
 706         struct ifnet *ifp = ifq->ifcq_ifp;
 707         u_int64_t rate, old_rate;
 708
 709         IFCQ_LOCK_ASSERT_HELD(ifq);
 710         VERIFY(IFCQ_IS_READY(ifq));
 711
 712         VERIFY(machclk_freq != 0);
 713
 714         tbr = &ifq->ifcq_tbr;
 715         old_rate = tbr->tbr_rate_raw;
 716
 717         rate = profile->rate;
 718         if (profile->percent > 0) {
 719                 u_int64_t eff_rate;
 720
 721                 if (profile->percent > 100)
 722                         return (EINVAL);
 723                 if ((eff_rate = ifp->if_output_bw.eff_bw) == 0)
 724                         return (ENODEV);
 725                 rate = (eff_rate * profile->percent) / 100;
 726         }
 727
 728         if (rate == 0) {
 729                 if (!IFCQ_TBR_IS_ENABLED(ifq))
 730                         return (ENOENT);
 731
 732                 if (pktsched_verbose)
 733                         printf("%s: TBR disabled\n", if_name(ifp));
 734
 735                 /* disable this TBR */
 736                 ifq->ifcq_flags &= ~IFCQF_TBR;
 737                 bzero(tbr, sizeof (*tbr));
 738                 ifnet_set_start_cycle(ifp, NULL);
 739                 if (update)
 740                         ifclassq_update(ifq, CLASSQ_EV_LINK_BANDWIDTH);
 741                 return (0);
 742         }
 743
 744         if (pktsched_verbose) {
 745                 printf("%s: TBR %s (rate %llu bps depth %u)\n", if_name(ifp),
 746                     (ifq->ifcq_flags & IFCQF_TBR) ? "reconfigured" :
 747                     "enabled", rate, profile->depth);
 748         }
 749
 750         /* set the new TBR */
 751         bzero(tbr, sizeof (*tbr));
 752         tbr->tbr_rate_raw = rate;
 753         tbr->tbr_percent = profile->percent;
 754         ifq->ifcq_flags |= IFCQF_TBR;
 755
 756         /*
 757          * Note that the TBR fill up time (hence the ifnet restart time)
 758          * is directly related to the specified TBR depth.  The ideal
 759          * depth value should be computed such that the interval time
 760          * between each successive wakeup is adequately spaced apart,
 761          * in order to reduce scheduling overheads.  A target interval
 762          * of 10 ms seems to provide good performance balance.  This can be
 763          * overridden by specifying the depth profile.  Values smaller than
 764          * the ideal depth will reduce delay at the expense of CPU cycles.
 765          */
 766         tbr->tbr_rate = TBR_SCALE(rate / 8) / machclk_freq;
 767         if (tbr->tbr_rate > 0) {
 768                 u_int32_t mtu = ifp->if_mtu;
 769                 int64_t ival, idepth = 0;
 770                 int i;
 771
 772                 if (mtu < IF_MINMTU)
 773                         mtu = IF_MINMTU;
 774
 775                 ival = pktsched_nsecs_to_abstime(10 * NSEC_PER_MSEC); /* 10ms */
 776
 777                 for (i = 1; ; i++) {
 778                         idepth = TBR_SCALE(i * mtu);
 779                         if ((idepth / tbr->tbr_rate) > ival)
 780                                 break;
 781                 }
 782                 VERIFY(idepth > 0);
 783
 784                 tbr->tbr_depth = TBR_SCALE(profile->depth);
 785                 if (tbr->tbr_depth == 0) {
 786                         tbr->tbr_filluptime = idepth / tbr->tbr_rate;
 787                         /* a little fudge factor to get closer to rate */
 788                         tbr->tbr_depth = idepth + (idepth >> 3);
 789                 } else {
 790                         tbr->tbr_filluptime = tbr->tbr_depth / tbr->tbr_rate;
 791                 }
 792         } else {
 793                 tbr->tbr_depth = TBR_SCALE(profile->depth);
 794                 tbr->tbr_filluptime = 0xffffffffffffffffLL;
 795         }
 796         tbr->tbr_token = tbr->tbr_depth;
 797         tbr->tbr_last = read_machclk();
 798         tbr->tbr_lastop = CLASSQDQ_REMOVE;
 799
 800         if (tbr->tbr_rate > 0 && (ifp->if_flags & IFF_UP)) {
 801                 struct timespec ts =
 802                     { 0, pktsched_abs_to_nsecs(tbr->tbr_filluptime) };
 803                 if (pktsched_verbose) {
 804                         printf("%s: TBR calculated tokens %lld "
 805                             "filluptime %llu ns\n", if_name(ifp),
 806                             TBR_UNSCALE(tbr->tbr_token),
 807                             pktsched_abs_to_nsecs(tbr->tbr_filluptime));
 808                 }
 809                 ifnet_set_start_cycle(ifp, &ts);
 810         } else {
 811                 if (pktsched_verbose) {
 812                         if (tbr->tbr_rate == 0) {
 813                                 printf("%s: TBR calculated tokens %lld "
 814                                     "infinite filluptime\n", if_name(ifp),
 815                                     TBR_UNSCALE(tbr->tbr_token));
 816                         } else if (!(ifp->if_flags & IFF_UP)) {
 817                                 printf("%s: TBR suspended (link is down)\n",
 818                                     if_name(ifp));
 819                         }
 820                 }
 821                 ifnet_set_start_cycle(ifp, NULL);
 822         }
 823         if (update && tbr->tbr_rate_raw != old_rate)
 824                 ifclassq_update(ifq, CLASSQ_EV_LINK_BANDWIDTH);
 825
 826         return (0);
 827 }