bsd/net/classq/classq.c

   1 /*
   2  * Copyright (c) 2007-2019 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  * Copyright (c) 1991-1997 Regents of the University of California.
  31  * All rights reserved.
  32  *
  33  * Redistribution and use in source and binary forms, with or without
  34  * modification, are permitted provided that the following conditions
  35  * are met:
  36  * 1. Redistributions of source code must retain the above copyright
  37  *    notice, this list of conditions and the following disclaimer.
  38  * 2. Redistributions in binary form must reproduce the above copyright
  39  *    notice, this list of conditions and the following disclaimer in the
  40  *    documentation and/or other materials provided with the distribution.
  41  * 3. All advertising materials mentioning features or use of this software
  42  *    must display the following acknowledgement:
  43  *      This product includes software developed by the Network Research
  44  *      Group at Lawrence Berkeley Laboratory.
  45  * 4. Neither the name of the University nor of the Laboratory may be used
  46  *    to endorse or promote products derived from this software without
  47  *    specific prior written permission.
  48  *
  49  * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
  50  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  51  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  52  * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
  53  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  54  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  55  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  56  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  57  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  58  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  59  * SUCH DAMAGE.
  60  */
  61
  62 #include <sys/cdefs.h>
  63 #include <sys/param.h>
  64 #include <sys/mbuf.h>
  65 #include <sys/errno.h>
  66 #include <sys/random.h>
  67 #include <sys/kernel_types.h>
  68 #include <sys/sysctl.h>
  69
  70 #include <net/if.h>
  71 #include <net/net_osdep.h>
  72 #include <net/classq/classq.h>
  73
  74 #include <libkern/libkern.h>
  75
  76
  77 u_int32_t classq_verbose = 0;   /* more noise if greater than 1 */
  78
  79 SYSCTL_NODE(_net, OID_AUTO, classq, CTLFLAG_RW | CTLFLAG_LOCKED, 0, "classq");
  80
  81 SYSCTL_UINT(_net_classq, OID_AUTO, verbose, CTLFLAG_RW | CTLFLAG_LOCKED,
  82     &classq_verbose, 0, "Class queue verbosity level");
  83
  84 void
  85 _qinit(class_queue_t *q, int type, int lim, classq_pkt_type_t ptype)
  86 {
  87         switch (ptype) {
  88         case QP_MBUF:
  89                 MBUFQ_INIT(&qmbufq(q));
  90                 break;
  91
  92
  93         default:
  94                 VERIFY(0);
  95                 /* NOTREACHED */
  96         }
  97
  98         qlimit(q) = lim;
  99         qlen(q) = 0;
 100         qsize(q) = 0;
 101         qtype(q) = type;
 102         qptype(q) = ptype;
 103         qstate(q) = QS_RUNNING;
 104 }
 105
 106 /* add a packet at the tail of the queue */
 107 void
 108 _addq(class_queue_t *q, classq_pkt_t *pkt)
 109 {
 110         uint32_t size = 0;
 111
 112         ASSERT(pkt->cp_ptype == qptype(q));
 113
 114         switch (qptype(q)) {
 115         case QP_MBUF: {
 116                 struct mbuf *m = pkt->cp_mbuf;
 117                 MBUFQ_ENQUEUE(&qmbufq(q), m);
 118                 size = m_length(m);
 119                 break;
 120         }
 121
 122
 123         default:
 124                 VERIFY(0);
 125                 /* NOTREACHED */
 126                 __builtin_unreachable();
 127         }
 128
 129         qlen(q)++;
 130         VERIFY(qlen(q) != 0);
 131         qsize(q) += size;
 132 }
 133
 134 /* add one or more packets at the tail of the queue */
 135 void
 136 _addq_multi(class_queue_t *q, classq_pkt_t *pkt_head, classq_pkt_t *pkt_tail,
 137     u_int32_t cnt, u_int32_t size)
 138 {
 139         ASSERT(pkt_head->cp_ptype == qptype(q));
 140         ASSERT(pkt_tail->cp_ptype == qptype(q));
 141         switch (qptype(q)) {
 142         case QP_MBUF: {
 143                 struct mbuf *m_head = pkt_head->cp_mbuf;
 144                 struct mbuf *m_tail = pkt_tail->cp_mbuf;
 145                 MBUFQ_ENQUEUE_MULTI(&qmbufq(q), m_head, m_tail);
 146                 break;
 147         }
 148
 149
 150         default:
 151                 VERIFY(0);
 152                 /* NOTREACHED */
 153                 __builtin_unreachable();
 154         }
 155
 156         qlen(q) += cnt;
 157         qsize(q) += size;
 158 }
 159
 160 /* get a packet at the head of the queue */
 161 void
 162 _getq(class_queue_t *q, classq_pkt_t *pkt)
 163 {
 164         uint32_t pkt_len;
 165
 166         switch (qptype(q)) {
 167         case QP_MBUF: {
 168                 MBUFQ_DEQUEUE(&qmbufq(q), pkt->cp_mbuf);
 169                 if (__probable(pkt->cp_mbuf != NULL)) {
 170                         CLASSQ_PKT_INIT_MBUF(pkt, pkt->cp_mbuf);
 171                         pkt_len = m_length(pkt->cp_mbuf);
 172                 }
 173                 break;
 174         }
 175
 176
 177         default:
 178                 VERIFY(0);
 179                 /* NOTREACHED */
 180                 __builtin_unreachable();
 181         }
 182
 183         if (pkt->cp_mbuf == NULL) {
 184                 VERIFY(qlen(q) == 0);
 185                 if (qsize(q) > 0) {
 186                         qsize(q) = 0;
 187                 }
 188                 return;
 189         }
 190         VERIFY(qlen(q) > 0);
 191         qlen(q)--;
 192
 193         /* qsize is an approximation, so adjust if necessary */
 194         if (((int)qsize(q) - pkt_len) > 0) {
 195                 qsize(q) -= pkt_len;
 196         } else if (qsize(q) != 0) {
 197                 qsize(q) = 0;
 198         }
 199 }
 200
 201 static void
 202 _getq_flow_or_scidx(class_queue_t *q, classq_pkt_t *pkt, u_int32_t val,
 203     boolean_t isflowid)
 204 {
 205         uint32_t pkt_len;
 206
 207         switch (qptype(q)) {
 208         case QP_MBUF: {
 209                 struct mbuf *m, *m_tmp;
 210
 211                 MBUFQ_FOREACH_SAFE(m, &qmbufq(q), m_tmp) {
 212                         if ((isflowid && (val == 0 ||
 213                             ((m->m_flags & M_PKTHDR) &&
 214                             m->m_pkthdr.pkt_flowid == val))) ||
 215                             (!isflowid &&
 216                             MBUF_SCIDX(mbuf_get_service_class(m)) < val)) {
 217                                 /* remove it from the class queue */
 218                                 MBUFQ_REMOVE(&qmbufq(q), m);
 219                                 MBUFQ_NEXT(m) = NULL;
 220                                 break;
 221                         }
 222                 }
 223                 if (__probable(m != NULL)) {
 224                         CLASSQ_PKT_INIT_MBUF(pkt, m);
 225                         pkt_len = m_length(m);
 226                 }
 227                 break;
 228         }
 229
 230
 231         default:
 232                 VERIFY(0);
 233                 /* NOTREACHED */
 234                 __builtin_unreachable();
 235         }
 236
 237         if (pkt->cp_mbuf != NULL) {
 238                 VERIFY(qlen(q) > 0);
 239                 qlen(q)--;
 240
 241                 /* qsize is an approximation, so adjust if necessary */
 242                 if (((int)qsize(q) - pkt_len) > 0) {
 243                         qsize(q) -= pkt_len;
 244                 } else if (qsize(q) != 0) {
 245                         qsize(q) = 0;
 246                 }
 247         }
 248 }
 249
 250 /* get a packet of a specific flow beginning from the head of the queue */
 251 void
 252 _getq_flow(class_queue_t *q, classq_pkt_t *pkt, u_int32_t flow)
 253 {
 254         return _getq_flow_or_scidx(q, pkt, flow, TRUE);
 255 }
 256
 257 /* Get a packet whose MBUF_SCIDX() < scidx from head of queue */
 258 void
 259 _getq_scidx_lt(class_queue_t *q, classq_pkt_t *pkt, u_int32_t scidx)
 260 {
 261         return _getq_flow_or_scidx(q, pkt, scidx, FALSE);
 262 }
 263
 264 /* get all packets (chained) starting from the head of the queue */
 265 void
 266 _getq_all(class_queue_t *q, classq_pkt_t *first, classq_pkt_t *last,
 267     u_int32_t *qlenp, u_int64_t *qsizep)
 268 {
 269         switch (qptype(q)) {
 270         case QP_MBUF:
 271                 first->cp_mbuf = MBUFQ_FIRST(&qmbufq(q));
 272                 if (__probable(first->cp_mbuf != NULL)) {
 273                         CLASSQ_PKT_INIT_MBUF(first, first->cp_mbuf);
 274                 }
 275                 if (last != NULL) {
 276                         last->cp_mbuf = MBUFQ_LAST(&qmbufq(q));
 277                         if (__probable(last->cp_mbuf != NULL)) {
 278                                 CLASSQ_PKT_INIT_MBUF(last, last->cp_mbuf);
 279                         }
 280                 }
 281                 MBUFQ_INIT(&qmbufq(q));
 282                 break;
 283
 284
 285         default:
 286                 VERIFY(0);
 287                 /* NOTREACHED */
 288                 __builtin_unreachable();
 289         }
 290
 291         if (qlenp != NULL) {
 292                 *qlenp = qlen(q);
 293         }
 294         if (qsizep != NULL) {
 295                 *qsizep = qsize(q);
 296         }
 297
 298         qlen(q) = 0;
 299         qsize(q) = 0;
 300 }
 301
 302 static inline struct mbuf *
 303 _getq_tail_mbuf(class_queue_t *q)
 304 {
 305         struct mq_head *head = &qmbufq(q);
 306         struct mbuf *m = MBUFQ_LAST(head);
 307
 308         if (m != NULL) {
 309                 struct mbuf *n = MBUFQ_FIRST(head);
 310
 311                 while (n != NULL) {
 312                         struct mbuf *next = MBUFQ_NEXT(n);
 313                         if (next == m) {
 314                                 MBUFQ_NEXT(n) = NULL;
 315                                 break;
 316                         }
 317                         n = next;
 318                 }
 319                 VERIFY(n != NULL ||
 320                     (qlen(q) == 1 && m == MBUFQ_FIRST(head)));
 321                 VERIFY(qlen(q) > 0);
 322                 --qlen(q);
 323
 324                 /* qsize is an approximation, so adjust if necessary */
 325                 if (((int)qsize(q) - m_length(m)) > 0) {
 326                         qsize(q) -= m_length(m);
 327                 } else if (qsize(q) != 0) {
 328                         qsize(q) = 0;
 329                 }
 330
 331                 if (qempty(q)) {
 332                         VERIFY(m == MBUFQ_FIRST(head));
 333                         MBUFQ_INIT(head);
 334                 } else {
 335                         VERIFY(n != NULL);
 336                         head->mq_last = &MBUFQ_NEXT(n);
 337                 }
 338         }
 339         return m;
 340 }
 341
 342 /* drop a packet at the tail of the queue */
 343 void
 344 _getq_tail(class_queue_t *q, classq_pkt_t *pkt)
 345 {
 346         switch (qptype(q)) {
 347         case QP_MBUF:
 348                 pkt->cp_mbuf = _getq_tail_mbuf(q);
 349                 if (__probable(pkt->cp_mbuf != NULL)) {
 350                         CLASSQ_PKT_INIT_MBUF(pkt, pkt->cp_mbuf);
 351                 }
 352                 break;
 353
 354         default:
 355                 VERIFY(0);
 356                 /* NOTREACHED */
 357                 __builtin_unreachable();
 358         }
 359 }
 360
 361 static inline struct mbuf *
 362 _getq_random_mbuf(class_queue_t *q)
 363 {
 364         struct mq_head *head = &qmbufq(q);
 365         struct mbuf *m = NULL;
 366         unsigned int n;
 367         u_int32_t rnd;
 368
 369         /* XXX: Add support for Kernel packet when needed */
 370         VERIFY((qptype(q) == QP_MBUF));
 371
 372         n = qlen(q);
 373         if (n == 0) {
 374                 VERIFY(MBUFQ_EMPTY(head));
 375                 if (qsize(q) > 0) {
 376                         qsize(q) = 0;
 377                 }
 378                 return NULL;
 379         }
 380
 381         m = MBUFQ_FIRST(head);
 382         read_frandom(&rnd, sizeof(rnd));
 383         n = (rnd % n) + 1;
 384
 385         if (n == 1) {
 386                 if ((MBUFQ_FIRST(head) = MBUFQ_NEXT(m)) == NULL) {
 387                         (head)->mq_last = &MBUFQ_FIRST(head);
 388                 }
 389         } else {
 390                 struct mbuf *p = NULL;
 391
 392                 VERIFY(n > 1);
 393                 while (n--) {
 394                         if (MBUFQ_NEXT(m) == NULL) {
 395                                 break;
 396                         }
 397                         p = m;
 398                         m = MBUFQ_NEXT(m);
 399                 }
 400                 VERIFY(p != NULL && MBUFQ_NEXT(p) == m);
 401
 402                 if ((MBUFQ_NEXT(p) = MBUFQ_NEXT(m)) == NULL) {
 403                         (head)->mq_last = &MBUFQ_NEXT(p);
 404                 }
 405         }
 406
 407         VERIFY(qlen(q) > 0);
 408         --qlen(q);
 409
 410         /* qsize is an approximation, so adjust if necessary */
 411         if (((int)qsize(q) - m_length(m)) > 0) {
 412                 qsize(q) -= m_length(m);
 413         } else if (qsize(q) != 0) {
 414                 qsize(q) = 0;
 415         }
 416
 417         MBUFQ_NEXT(m) = NULL;
 418
 419         return m;
 420 }
 421
 422 /* randomly select a packet in the queue */
 423 void
 424 _getq_random(class_queue_t *q, classq_pkt_t *pkt)
 425 {
 426         switch (qptype(q)) {
 427         case QP_MBUF:
 428                 pkt->cp_mbuf = _getq_random_mbuf(q);
 429                 if (__probable(pkt->cp_mbuf != NULL)) {
 430                         CLASSQ_PKT_INIT_MBUF(pkt, pkt->cp_mbuf);
 431                 }
 432                 break;
 433
 434         default:
 435                 VERIFY(0);
 436                 /* NOTREACHED */
 437                 __builtin_unreachable();
 438         }
 439 }
 440
 441 static inline void
 442 _removeq_mbuf(class_queue_t *q, struct mbuf *m)
 443 {
 444         struct mq_head *head = &qmbufq(q);
 445         struct mbuf *m0, **mtail;
 446
 447         m0 = MBUFQ_FIRST(head);
 448         if (m0 == NULL) {
 449                 return;
 450         }
 451
 452         if (m0 != m) {
 453                 while (m0 != NULL && MBUFQ_NEXT(m0) != m) {
 454                         m0 = MBUFQ_NEXT(m0);
 455                 }
 456                 if (m0 == NULL) {
 457                         return;
 458                 }
 459
 460                 mtail = &MBUFQ_NEXT(m0);
 461         } else {
 462                 mtail = &MBUFQ_FIRST(head);
 463         }
 464
 465         *mtail = MBUFQ_NEXT(m);
 466         if (*mtail == NULL) {
 467                 head->mq_last = mtail;
 468         }
 469
 470         VERIFY(qlen(q) > 0);
 471         --qlen(q);
 472
 473         /* qsize is an approximation, so adjust if necessary */
 474         if (((int)qsize(q) - m_length(m)) > 0) {
 475                 qsize(q) -= m_length(m);
 476         } else if (qsize(q) != 0) {
 477                 qsize(q) = 0;
 478         }
 479
 480         MBUFQ_NEXT(m) = NULL;
 481 }
 482
 483 /* remove a packet from the queue */
 484 void
 485 _removeq(class_queue_t *q, classq_pkt_t *pkt)
 486 {
 487         switch (qptype(q)) {
 488         case QP_MBUF:
 489                 ASSERT(pkt->cp_ptype == QP_MBUF);
 490                 _removeq_mbuf(q, pkt->cp_mbuf);
 491                 break;
 492
 493         default:
 494                 VERIFY(0);
 495                 /* NOTREACHED */
 496                 __builtin_unreachable();
 497         }
 498 }
 499
 500 void
 501 _flushq(class_queue_t *q)
 502 {
 503         (void) _flushq_flow(q, 0, NULL, NULL);
 504 }
 505
 506 static inline void
 507 _flushq_flow_mbuf(class_queue_t *q, u_int32_t flow, u_int32_t *cnt,
 508     u_int32_t *len)
 509 {
 510         MBUFQ_HEAD(mq_freeq) freeq;
 511         struct mbuf *m, *m_tmp;
 512         u_int32_t c = 0, l = 0;
 513
 514         MBUFQ_INIT(&freeq);
 515
 516         MBUFQ_FOREACH_SAFE(m, &qmbufq(q), m_tmp) {
 517                 if (flow == 0 || ((m->m_flags & M_PKTHDR) &&
 518                     m->m_pkthdr.pkt_flowid == flow)) {
 519                         /* remove it from the class queue */
 520                         MBUFQ_REMOVE(&qmbufq(q), m);
 521                         MBUFQ_NEXT(m) = NULL;
 522
 523                         /* and add it to the free queue */
 524                         MBUFQ_ENQUEUE(&freeq, m);
 525
 526                         l += m_length(m);
 527                         c++;
 528                 }
 529         }
 530         VERIFY(c == 0 || !MBUFQ_EMPTY(&freeq));
 531
 532         if (c > 0) {
 533                 VERIFY(qlen(q) >= c);
 534                 qlen(q) -= c;
 535
 536                 /* qsize is an approximation, so adjust if necessary */
 537                 if (((int)qsize(q) - l) > 0) {
 538                         qsize(q) -= l;
 539                 } else if (qsize(q) != 0) {
 540                         qsize(q) = 0;
 541                 }
 542         }
 543
 544         if (!MBUFQ_EMPTY(&freeq)) {
 545                 m_freem_list(MBUFQ_FIRST(&freeq));
 546         }
 547
 548         if (cnt != NULL) {
 549                 *cnt = c;
 550         }
 551         if (len != NULL) {
 552                 *len = l;
 553         }
 554 }
 555
 556
 557 void
 558 _flushq_flow(class_queue_t *q, u_int32_t flow, u_int32_t *cnt, u_int32_t *len)
 559 {
 560         switch (qptype(q)) {
 561         case QP_MBUF:
 562                 _flushq_flow_mbuf(q, flow, cnt, len);
 563                 break;
 564
 565         default:
 566                 VERIFY(0);
 567                 /* NOTREACHED */
 568                 __builtin_unreachable();
 569         }
 570 }