sys/net/altq/altq_priq.c

   1 /*-
   2  * Copyright (C) 2000-2003
   3  *      Sony Computer Science Laboratories Inc.  All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   8  * 1. Redistributions of source code must retain the above copyright
   9  *    notice, this list of conditions and the following disclaimer.
  10  * 2. Redistributions in binary form must reproduce the above copyright
  11  *    notice, this list of conditions and the following disclaimer in the
  12  *    documentation and/or other materials provided with the distribution.
  13  *
  14  * THIS SOFTWARE IS PROVIDED BY SONY CSL AND CONTRIBUTORS ``AS IS'' AND
  15  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  17  * ARE DISCLAIMED.  IN NO EVENT SHALL SONY CSL OR CONTRIBUTORS BE LIABLE
  18  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  19  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  20  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  21  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  22  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  23  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  24  * SUCH DAMAGE.
  25  *
  26  * $KAME: altq_priq.c,v 1.11 2003/09/17 14:23:25 kjc Exp $
  27  * $FreeBSD$
  28  */
  29 /*
  30  * priority queue
  31  */
  32
  33 #include "opt_altq.h"
  34 #include "opt_inet.h"
  35 #include "opt_inet6.h"
  36
  37 #ifdef ALTQ_PRIQ  /* priq is enabled by ALTQ_PRIQ option in opt_altq.h */
  38
  39 #include <sys/param.h>
  40 #include <sys/malloc.h>
  41 #include <sys/mbuf.h>
  42 #include <sys/socket.h>
  43 #include <sys/sockio.h>
  44 #include <sys/systm.h>
  45 #include <sys/proc.h>
  46 #include <sys/errno.h>
  47 #include <sys/kernel.h>
  48 #include <sys/queue.h>
  49
  50 #include <net/if.h>
  51 #include <net/if_var.h>
  52 #include <netinet/in.h>
  53
  54 #include <netpfil/pf/pf.h>
  55 #include <netpfil/pf/pf_altq.h>
  56 #include <netpfil/pf/pf_mtag.h>
  57 #include <net/altq/altq.h>
  58 #ifdef ALTQ3_COMPAT
  59 #include <net/altq/altq_conf.h>
  60 #endif
  61 #include <net/altq/altq_priq.h>
  62
  63 /*
  64  * function prototypes
  65  */
  66 #ifdef ALTQ3_COMPAT
  67 static struct priq_if *priq_attach(struct ifaltq *, u_int);
  68 static int priq_detach(struct priq_if *);
  69 #endif
  70 static int priq_clear_interface(struct priq_if *);
  71 static int priq_request(struct ifaltq *, int, void *);
  72 static void priq_purge(struct priq_if *);
  73 static struct priq_class *priq_class_create(struct priq_if *, int, int, int,
  74     int);
  75 static int priq_class_destroy(struct priq_class *);
  76 static int priq_enqueue(struct ifaltq *, struct mbuf *, struct altq_pktattr *);
  77 static struct mbuf *priq_dequeue(struct ifaltq *, int);
  78
  79 static int priq_addq(struct priq_class *, struct mbuf *);
  80 static struct mbuf *priq_getq(struct priq_class *);
  81 static struct mbuf *priq_pollq(struct priq_class *);
  82 static void priq_purgeq(struct priq_class *);
  83
  84 #ifdef ALTQ3_COMPAT
  85 static int priqcmd_if_attach(struct priq_interface *);
  86 static int priqcmd_if_detach(struct priq_interface *);
  87 static int priqcmd_add_class(struct priq_add_class *);
  88 static int priqcmd_delete_class(struct priq_delete_class *);
  89 static int priqcmd_modify_class(struct priq_modify_class *);
  90 static int priqcmd_add_filter(struct priq_add_filter *);
  91 static int priqcmd_delete_filter(struct priq_delete_filter *);
  92 static int priqcmd_class_stats(struct priq_class_stats *);
  93 #endif /* ALTQ3_COMPAT */
  94
  95 static void get_class_stats(struct priq_classstats *, struct priq_class *);
  96 static struct priq_class *clh_to_clp(struct priq_if *, u_int32_t);
  97
  98 #ifdef ALTQ3_COMPAT
  99 altqdev_decl(priq);
 100
 101 /* pif_list keeps all priq_if's allocated. */
 102 static struct priq_if *pif_list = NULL;
 103 #endif /* ALTQ3_COMPAT */
 104
 105 int
 106 priq_pfattach(struct pf_altq *a)
 107 {
 108         struct ifnet *ifp;
 109         int s, error;
 110
 111         if ((ifp = ifunit(a->ifname)) == NULL || a->altq_disc == NULL)
 112                 return (EINVAL);
 113         s = splnet();
 114         error = altq_attach(&ifp->if_snd, ALTQT_PRIQ, a->altq_disc,
 115             priq_enqueue, priq_dequeue, priq_request, NULL, NULL);
 116         splx(s);
 117         return (error);
 118 }
 119
 120 int
 121 priq_add_altq(struct pf_altq *a)
 122 {
 123         struct priq_if  *pif;
 124         struct ifnet    *ifp;
 125
 126         if ((ifp = ifunit(a->ifname)) == NULL)
 127                 return (EINVAL);
 128         if (!ALTQ_IS_READY(&ifp->if_snd))
 129                 return (ENODEV);
 130
 131         pif = malloc(sizeof(struct priq_if), M_DEVBUF, M_NOWAIT | M_ZERO);
 132         if (pif == NULL)
 133                 return (ENOMEM);
 134         pif->pif_bandwidth = a->ifbandwidth;
 135         pif->pif_maxpri = -1;
 136         pif->pif_ifq = &ifp->if_snd;
 137
 138         /* keep the state in pf_altq */
 139         a->altq_disc = pif;
 140
 141         return (0);
 142 }
 143
 144 int
 145 priq_remove_altq(struct pf_altq *a)
 146 {
 147         struct priq_if *pif;
 148
 149         if ((pif = a->altq_disc) == NULL)
 150                 return (EINVAL);
 151         a->altq_disc = NULL;
 152
 153         (void)priq_clear_interface(pif);
 154
 155         free(pif, M_DEVBUF);
 156         return (0);
 157 }
 158
 159 int
 160 priq_add_queue(struct pf_altq *a)
 161 {
 162         struct priq_if *pif;
 163         struct priq_class *cl;
 164
 165         if ((pif = a->altq_disc) == NULL)
 166                 return (EINVAL);
 167
 168         /* check parameters */
 169         if (a->priority >= PRIQ_MAXPRI)
 170                 return (EINVAL);
 171         if (a->qid == 0)
 172                 return (EINVAL);
 173         if (pif->pif_classes[a->priority] != NULL)
 174                 return (EBUSY);
 175         if (clh_to_clp(pif, a->qid) != NULL)
 176                 return (EBUSY);
 177
 178         cl = priq_class_create(pif, a->priority, a->qlimit,
 179             a->pq_u.priq_opts.flags, a->qid);
 180         if (cl == NULL)
 181                 return (ENOMEM);
 182
 183         return (0);
 184 }
 185
 186 int
 187 priq_remove_queue(struct pf_altq *a)
 188 {
 189         struct priq_if *pif;
 190         struct priq_class *cl;
 191
 192         if ((pif = a->altq_disc) == NULL)
 193                 return (EINVAL);
 194
 195         if ((cl = clh_to_clp(pif, a->qid)) == NULL)
 196                 return (EINVAL);
 197
 198         return (priq_class_destroy(cl));
 199 }
 200
 201 int
 202 priq_getqstats(struct pf_altq *a, void *ubuf, int *nbytes)
 203 {
 204         struct priq_if *pif;
 205         struct priq_class *cl;
 206         struct priq_classstats stats;
 207         int error = 0;
 208
 209         if ((pif = altq_lookup(a->ifname, ALTQT_PRIQ)) == NULL)
 210                 return (EBADF);
 211
 212         if ((cl = clh_to_clp(pif, a->qid)) == NULL)
 213                 return (EINVAL);
 214
 215         if (*nbytes < sizeof(stats))
 216                 return (EINVAL);
 217
 218         get_class_stats(&stats, cl);
 219
 220         if ((error = copyout((caddr_t)&stats, ubuf, sizeof(stats))) != 0)
 221                 return (error);
 222         *nbytes = sizeof(stats);
 223         return (0);
 224 }
 225
 226 /*
 227  * bring the interface back to the initial state by discarding
 228  * all the filters and classes.
 229  */
 230 static int
 231 priq_clear_interface(struct priq_if *pif)
 232 {
 233         struct priq_class       *cl;
 234         int pri;
 235
 236 #ifdef ALTQ3_CLFIER_COMPAT
 237         /* free the filters for this interface */
 238         acc_discard_filters(&pif->pif_classifier, NULL, 1);
 239 #endif
 240
 241         /* clear out the classes */
 242         for (pri = 0; pri <= pif->pif_maxpri; pri++)
 243                 if ((cl = pif->pif_classes[pri]) != NULL)
 244                         priq_class_destroy(cl);
 245
 246         return (0);
 247 }
 248
 249 static int
 250 priq_request(struct ifaltq *ifq, int req, void *arg)
 251 {
 252         struct priq_if  *pif = (struct priq_if *)ifq->altq_disc;
 253
 254         IFQ_LOCK_ASSERT(ifq);
 255
 256         switch (req) {
 257         case ALTRQ_PURGE:
 258                 priq_purge(pif);
 259                 break;
 260         }
 261         return (0);
 262 }
 263
 264 /* discard all the queued packets on the interface */
 265 static void
 266 priq_purge(struct priq_if *pif)
 267 {
 268         struct priq_class *cl;
 269         int pri;
 270
 271         for (pri = 0; pri <= pif->pif_maxpri; pri++) {
 272                 if ((cl = pif->pif_classes[pri]) != NULL && !qempty(cl->cl_q))
 273                         priq_purgeq(cl);
 274         }
 275         if (ALTQ_IS_ENABLED(pif->pif_ifq))
 276                 pif->pif_ifq->ifq_len = 0;
 277 }
 278
 279 static struct priq_class *
 280 priq_class_create(struct priq_if *pif, int pri, int qlimit, int flags, int qid)
 281 {
 282         struct priq_class *cl;
 283         int s;
 284
 285 #ifndef ALTQ_RED
 286         if (flags & PRCF_RED) {
 287 #ifdef ALTQ_DEBUG
 288                 printf("priq_class_create: RED not configured for PRIQ!\n");
 289 #endif
 290                 return (NULL);
 291         }
 292 #endif
 293 #ifndef ALTQ_CODEL
 294         if (flags & PRCF_CODEL) {
 295 #ifdef ALTQ_DEBUG
 296                 printf("priq_class_create: CODEL not configured for PRIQ!\n");
 297 #endif
 298                 return (NULL);
 299         }
 300 #endif
 301
 302         if ((cl = pif->pif_classes[pri]) != NULL) {
 303                 /* modify the class instead of creating a new one */
 304                 s = splnet();
 305                 IFQ_LOCK(cl->cl_pif->pif_ifq);
 306                 if (!qempty(cl->cl_q))
 307                         priq_purgeq(cl);
 308                 IFQ_UNLOCK(cl->cl_pif->pif_ifq);
 309                 splx(s);
 310 #ifdef ALTQ_RIO
 311                 if (q_is_rio(cl->cl_q))
 312                         rio_destroy((rio_t *)cl->cl_red);
 313 #endif
 314 #ifdef ALTQ_RED
 315                 if (q_is_red(cl->cl_q))
 316                         red_destroy(cl->cl_red);
 317 #endif
 318 #ifdef ALTQ_CODEL
 319                 if (q_is_codel(cl->cl_q))
 320                         codel_destroy(cl->cl_codel);
 321 #endif
 322         } else {
 323                 cl = malloc(sizeof(struct priq_class), M_DEVBUF,
 324                     M_NOWAIT | M_ZERO);
 325                 if (cl == NULL)
 326                         return (NULL);
 327
 328                 cl->cl_q = malloc(sizeof(class_queue_t), M_DEVBUF,
 329                     M_NOWAIT | M_ZERO);
 330                 if (cl->cl_q == NULL)
 331                         goto err_ret;
 332         }
 333
 334         pif->pif_classes[pri] = cl;
 335         if (flags & PRCF_DEFAULTCLASS)
 336                 pif->pif_default = cl;
 337         if (qlimit == 0)
 338                 qlimit = 50;  /* use default */
 339         qlimit(cl->cl_q) = qlimit;
 340         qtype(cl->cl_q) = Q_DROPTAIL;
 341         qlen(cl->cl_q) = 0;
 342         qsize(cl->cl_q) = 0;
 343         cl->cl_flags = flags;
 344         cl->cl_pri = pri;
 345         if (pri > pif->pif_maxpri)
 346                 pif->pif_maxpri = pri;
 347         cl->cl_pif = pif;
 348         cl->cl_handle = qid;
 349
 350 #ifdef ALTQ_RED
 351         if (flags & (PRCF_RED|PRCF_RIO)) {
 352                 int red_flags, red_pkttime;
 353
 354                 red_flags = 0;
 355                 if (flags & PRCF_ECN)
 356                         red_flags |= REDF_ECN;
 357 #ifdef ALTQ_RIO
 358                 if (flags & PRCF_CLEARDSCP)
 359                         red_flags |= RIOF_CLEARDSCP;
 360 #endif
 361                 if (pif->pif_bandwidth < 8)
 362                         red_pkttime = 1000 * 1000 * 1000; /* 1 sec */
 363                 else
 364                         red_pkttime = (int64_t)pif->pif_ifq->altq_ifp->if_mtu
 365                           * 1000 * 1000 * 1000 / (pif->pif_bandwidth / 8);
 366 #ifdef ALTQ_RIO
 367                 if (flags & PRCF_RIO) {
 368                         cl->cl_red = (red_t *)rio_alloc(0, NULL,
 369                                                 red_flags, red_pkttime);
 370                         if (cl->cl_red == NULL)
 371                                 goto err_ret;
 372                         qtype(cl->cl_q) = Q_RIO;
 373                 } else
 374 #endif
 375                 if (flags & PRCF_RED) {
 376                         cl->cl_red = red_alloc(0, 0,
 377                             qlimit(cl->cl_q) * 10/100,
 378                             qlimit(cl->cl_q) * 30/100,
 379                             red_flags, red_pkttime);
 380                         if (cl->cl_red == NULL)
 381                                 goto err_ret;
 382                         qtype(cl->cl_q) = Q_RED;
 383                 }
 384         }
 385 #endif /* ALTQ_RED */
 386 #ifdef ALTQ_CODEL
 387         if (flags & PRCF_CODEL) {
 388                 cl->cl_codel = codel_alloc(5, 100, 0);
 389                 if (cl->cl_codel != NULL)
 390                         qtype(cl->cl_q) = Q_CODEL;
 391         }
 392 #endif
 393
 394         return (cl);
 395
 396  err_ret:
 397         if (cl->cl_red != NULL) {
 398 #ifdef ALTQ_RIO
 399                 if (q_is_rio(cl->cl_q))
 400                         rio_destroy((rio_t *)cl->cl_red);
 401 #endif
 402 #ifdef ALTQ_RED
 403                 if (q_is_red(cl->cl_q))
 404                         red_destroy(cl->cl_red);
 405 #endif
 406 #ifdef ALTQ_CODEL
 407                 if (q_is_codel(cl->cl_q))
 408                         codel_destroy(cl->cl_codel);
 409 #endif
 410         }
 411         if (cl->cl_q != NULL)
 412                 free(cl->cl_q, M_DEVBUF);
 413         free(cl, M_DEVBUF);
 414         return (NULL);
 415 }
 416
 417 static int
 418 priq_class_destroy(struct priq_class *cl)
 419 {
 420         struct priq_if *pif;
 421         int s, pri;
 422
 423         s = splnet();
 424         IFQ_LOCK(cl->cl_pif->pif_ifq);
 425
 426 #ifdef ALTQ3_CLFIER_COMPAT
 427         /* delete filters referencing to this class */
 428         acc_discard_filters(&cl->cl_pif->pif_classifier, cl, 0);
 429 #endif
 430
 431         if (!qempty(cl->cl_q))
 432                 priq_purgeq(cl);
 433
 434         pif = cl->cl_pif;
 435         pif->pif_classes[cl->cl_pri] = NULL;
 436         if (pif->pif_maxpri == cl->cl_pri) {
 437                 for (pri = cl->cl_pri; pri >= 0; pri--)
 438                         if (pif->pif_classes[pri] != NULL) {
 439                                 pif->pif_maxpri = pri;
 440                                 break;
 441                         }
 442                 if (pri < 0)
 443                         pif->pif_maxpri = -1;
 444         }
 445         IFQ_UNLOCK(cl->cl_pif->pif_ifq);
 446         splx(s);
 447
 448         if (cl->cl_red != NULL) {
 449 #ifdef ALTQ_RIO
 450                 if (q_is_rio(cl->cl_q))
 451                         rio_destroy((rio_t *)cl->cl_red);
 452 #endif
 453 #ifdef ALTQ_RED
 454                 if (q_is_red(cl->cl_q))
 455                         red_destroy(cl->cl_red);
 456 #endif
 457 #ifdef ALTQ_CODEL
 458                 if (q_is_codel(cl->cl_q))
 459                         codel_destroy(cl->cl_codel);
 460 #endif
 461         }
 462         free(cl->cl_q, M_DEVBUF);
 463         free(cl, M_DEVBUF);
 464         return (0);
 465 }
 466
 467 /*
 468  * priq_enqueue is an enqueue function to be registered to
 469  * (*altq_enqueue) in struct ifaltq.
 470  */
 471 static int
 472 priq_enqueue(struct ifaltq *ifq, struct mbuf *m, struct altq_pktattr *pktattr)
 473 {
 474         struct priq_if  *pif = (struct priq_if *)ifq->altq_disc;
 475         struct priq_class *cl;
 476         struct pf_mtag *t;
 477         int len;
 478
 479         IFQ_LOCK_ASSERT(ifq);
 480
 481         /* grab class set by classifier */
 482         if ((m->m_flags & M_PKTHDR) == 0) {
 483                 /* should not happen */
 484                 printf("altq: packet for %s does not have pkthdr\n",
 485                     ifq->altq_ifp->if_xname);
 486                 m_freem(m);
 487                 return (ENOBUFS);
 488         }
 489         cl = NULL;
 490         if ((t = pf_find_mtag(m)) != NULL)
 491                 cl = clh_to_clp(pif, t->qid);
 492 #ifdef ALTQ3_COMPAT
 493         else if ((ifq->altq_flags & ALTQF_CLASSIFY) && pktattr != NULL)
 494                 cl = pktattr->pattr_class;
 495 #endif
 496         if (cl == NULL) {
 497                 cl = pif->pif_default;
 498                 if (cl == NULL) {
 499                         m_freem(m);
 500                         return (ENOBUFS);
 501                 }
 502         }
 503 #ifdef ALTQ3_COMPAT
 504         if (pktattr != NULL)
 505                 cl->cl_pktattr = pktattr;  /* save proto hdr used by ECN */
 506         else
 507 #endif
 508                 cl->cl_pktattr = NULL;
 509         len = m_pktlen(m);
 510         if (priq_addq(cl, m) != 0) {
 511                 /* drop occurred.  mbuf was freed in priq_addq. */
 512                 PKTCNTR_ADD(&cl->cl_dropcnt, len);
 513                 return (ENOBUFS);
 514         }
 515         IFQ_INC_LEN(ifq);
 516
 517         /* successfully queued. */
 518         return (0);
 519 }
 520
 521 /*
 522  * priq_dequeue is a dequeue function to be registered to
 523  * (*altq_dequeue) in struct ifaltq.
 524  *
 525  * note: ALTDQ_POLL returns the next packet without removing the packet
 526  *      from the queue.  ALTDQ_REMOVE is a normal dequeue operation.
 527  *      ALTDQ_REMOVE must return the same packet if called immediately
 528  *      after ALTDQ_POLL.
 529  */
 530 static struct mbuf *
 531 priq_dequeue(struct ifaltq *ifq, int op)
 532 {
 533         struct priq_if  *pif = (struct priq_if *)ifq->altq_disc;
 534         struct priq_class *cl;
 535         struct mbuf *m;
 536         int pri;
 537
 538         IFQ_LOCK_ASSERT(ifq);
 539
 540         if (IFQ_IS_EMPTY(ifq))
 541                 /* no packet in the queue */
 542                 return (NULL);
 543
 544         for (pri = pif->pif_maxpri;  pri >= 0; pri--) {
 545                 if ((cl = pif->pif_classes[pri]) != NULL &&
 546                     !qempty(cl->cl_q)) {
 547                         if (op == ALTDQ_POLL)
 548                                 return (priq_pollq(cl));
 549
 550                         m = priq_getq(cl);
 551                         if (m != NULL) {
 552                                 IFQ_DEC_LEN(ifq);
 553                                 if (qempty(cl->cl_q))
 554                                         cl->cl_period++;
 555                                 PKTCNTR_ADD(&cl->cl_xmitcnt, m_pktlen(m));
 556                         }
 557                         return (m);
 558                 }
 559         }
 560         return (NULL);
 561 }
 562
 563 static int
 564 priq_addq(struct priq_class *cl, struct mbuf *m)
 565 {
 566
 567 #ifdef ALTQ_RIO
 568         if (q_is_rio(cl->cl_q))
 569                 return rio_addq((rio_t *)cl->cl_red, cl->cl_q, m,
 570                                 cl->cl_pktattr);
 571 #endif
 572 #ifdef ALTQ_RED
 573         if (q_is_red(cl->cl_q))
 574                 return red_addq(cl->cl_red, cl->cl_q, m, cl->cl_pktattr);
 575 #endif
 576 #ifdef ALTQ_CODEL
 577         if (q_is_codel(cl->cl_q))
 578                 return codel_addq(cl->cl_codel, cl->cl_q, m);
 579 #endif
 580         if (qlen(cl->cl_q) >= qlimit(cl->cl_q)) {
 581                 m_freem(m);
 582                 return (-1);
 583         }
 584
 585         if (cl->cl_flags & PRCF_CLEARDSCP)
 586                 write_dsfield(m, cl->cl_pktattr, 0);
 587
 588         _addq(cl->cl_q, m);
 589
 590         return (0);
 591 }
 592
 593 static struct mbuf *
 594 priq_getq(struct priq_class *cl)
 595 {
 596 #ifdef ALTQ_RIO
 597         if (q_is_rio(cl->cl_q))
 598                 return rio_getq((rio_t *)cl->cl_red, cl->cl_q);
 599 #endif
 600 #ifdef ALTQ_RED
 601         if (q_is_red(cl->cl_q))
 602                 return red_getq(cl->cl_red, cl->cl_q);
 603 #endif
 604 #ifdef ALTQ_CODEL
 605         if (q_is_codel(cl->cl_q))
 606                 return codel_getq(cl->cl_codel, cl->cl_q);
 607 #endif
 608         return _getq(cl->cl_q);
 609 }
 610
 611 static struct mbuf *
 612 priq_pollq(cl)
 613         struct priq_class *cl;
 614 {
 615         return qhead(cl->cl_q);
 616 }
 617
 618 static void
 619 priq_purgeq(struct priq_class *cl)
 620 {
 621         struct mbuf *m;
 622
 623         if (qempty(cl->cl_q))
 624                 return;
 625
 626         while ((m = _getq(cl->cl_q)) != NULL) {
 627                 PKTCNTR_ADD(&cl->cl_dropcnt, m_pktlen(m));
 628                 m_freem(m);
 629         }
 630         ASSERT(qlen(cl->cl_q) == 0);
 631 }
 632
 633 static void
 634 get_class_stats(struct priq_classstats *sp, struct priq_class *cl)
 635 {
 636         sp->class_handle = cl->cl_handle;
 637         sp->qlength = qlen(cl->cl_q);
 638         sp->qlimit = qlimit(cl->cl_q);
 639         sp->period = cl->cl_period;
 640         sp->xmitcnt = cl->cl_xmitcnt;
 641         sp->dropcnt = cl->cl_dropcnt;
 642
 643         sp->qtype = qtype(cl->cl_q);
 644 #ifdef ALTQ_RED
 645         if (q_is_red(cl->cl_q))
 646                 red_getstats(cl->cl_red, &sp->red[0]);
 647 #endif
 648 #ifdef ALTQ_RIO
 649         if (q_is_rio(cl->cl_q))
 650                 rio_getstats((rio_t *)cl->cl_red, &sp->red[0]);
 651 #endif
 652 #ifdef ALTQ_CODEL
 653         if (q_is_codel(cl->cl_q))
 654                 codel_getstats(cl->cl_codel, &sp->codel);
 655 #endif
 656 }
 657
 658 /* convert a class handle to the corresponding class pointer */
 659 static struct priq_class *
 660 clh_to_clp(struct priq_if *pif, u_int32_t chandle)
 661 {
 662         struct priq_class *cl;
 663         int idx;
 664
 665         if (chandle == 0)
 666                 return (NULL);
 667
 668         for (idx = pif->pif_maxpri; idx >= 0; idx--)
 669                 if ((cl = pif->pif_classes[idx]) != NULL &&
 670                     cl->cl_handle == chandle)
 671                         return (cl);
 672
 673         return (NULL);
 674 }
 675
 676
 677 #ifdef ALTQ3_COMPAT
 678
 679 static struct priq_if *
 680 priq_attach(ifq, bandwidth)
 681         struct ifaltq *ifq;
 682         u_int bandwidth;
 683 {
 684         struct priq_if *pif;
 685
 686         pif = malloc(sizeof(struct priq_if),
 687                M_DEVBUF, M_WAITOK);
 688         if (pif == NULL)
 689                 return (NULL);
 690         bzero(pif, sizeof(struct priq_if));
 691         pif->pif_bandwidth = bandwidth;
 692         pif->pif_maxpri = -1;
 693         pif->pif_ifq = ifq;
 694
 695         /* add this state to the priq list */
 696         pif->pif_next = pif_list;
 697         pif_list = pif;
 698
 699         return (pif);
 700 }
 701
 702 static int
 703 priq_detach(pif)
 704         struct priq_if *pif;
 705 {
 706         (void)priq_clear_interface(pif);
 707
 708         /* remove this interface from the pif list */
 709         if (pif_list == pif)
 710                 pif_list = pif->pif_next;
 711         else {
 712                 struct priq_if *p;
 713
 714                 for (p = pif_list; p != NULL; p = p->pif_next)
 715                         if (p->pif_next == pif) {
 716                                 p->pif_next = pif->pif_next;
 717                                 break;
 718                         }
 719                 ASSERT(p != NULL);
 720         }
 721
 722         free(pif, M_DEVBUF);
 723         return (0);
 724 }
 725
 726 /*
 727  * priq device interface
 728  */
 729 int
 730 priqopen(dev, flag, fmt, p)
 731         dev_t dev;
 732         int flag, fmt;
 733 #if (__FreeBSD_version > 500000)
 734         struct thread *p;
 735 #else
 736         struct proc *p;
 737 #endif
 738 {
 739         /* everything will be done when the queueing scheme is attached. */
 740         return 0;
 741 }
 742
 743 int
 744 priqclose(dev, flag, fmt, p)
 745         dev_t dev;
 746         int flag, fmt;
 747 #if (__FreeBSD_version > 500000)
 748         struct thread *p;
 749 #else
 750         struct proc *p;
 751 #endif
 752 {
 753         struct priq_if *pif;
 754         int err, error = 0;
 755
 756         while ((pif = pif_list) != NULL) {
 757                 /* destroy all */
 758                 if (ALTQ_IS_ENABLED(pif->pif_ifq))
 759                         altq_disable(pif->pif_ifq);
 760
 761                 err = altq_detach(pif->pif_ifq);
 762                 if (err == 0)
 763                         err = priq_detach(pif);
 764                 if (err != 0 && error == 0)
 765                         error = err;
 766         }
 767
 768         return error;
 769 }
 770
 771 int
 772 priqioctl(dev, cmd, addr, flag, p)
 773         dev_t dev;
 774         ioctlcmd_t cmd;
 775         caddr_t addr;
 776         int flag;
 777 #if (__FreeBSD_version > 500000)
 778         struct thread *p;
 779 #else
 780         struct proc *p;
 781 #endif
 782 {
 783         struct priq_if *pif;
 784         struct priq_interface *ifacep;
 785         int     error = 0;
 786
 787         /* check super-user privilege */
 788         switch (cmd) {
 789         case PRIQ_GETSTATS:
 790                 break;
 791         default:
 792 #if (__FreeBSD_version > 700000)
 793                 if ((error = priv_check(p, PRIV_ALTQ_MANAGE)) != 0)
 794                         return (error);
 795 #elsif (__FreeBSD_version > 400000)
 796                 if ((error = suser(p)) != 0)
 797                         return (error);
 798 #else
 799                 if ((error = suser(p->p_ucred, &p->p_acflag)) != 0)
 800                         return (error);
 801 #endif
 802                 break;
 803         }
 804
 805         switch (cmd) {
 806
 807         case PRIQ_IF_ATTACH:
 808                 error = priqcmd_if_attach((struct priq_interface *)addr);
 809                 break;
 810
 811         case PRIQ_IF_DETACH:
 812                 error = priqcmd_if_detach((struct priq_interface *)addr);
 813                 break;
 814
 815         case PRIQ_ENABLE:
 816         case PRIQ_DISABLE:
 817         case PRIQ_CLEAR:
 818                 ifacep = (struct priq_interface *)addr;
 819                 if ((pif = altq_lookup(ifacep->ifname,
 820                                        ALTQT_PRIQ)) == NULL) {
 821                         error = EBADF;
 822                         break;
 823                 }
 824
 825                 switch (cmd) {
 826                 case PRIQ_ENABLE:
 827                         if (pif->pif_default == NULL) {
 828 #ifdef ALTQ_DEBUG
 829                                 printf("priq: no default class\n");
 830 #endif
 831                                 error = EINVAL;
 832                                 break;
 833                         }
 834                         error = altq_enable(pif->pif_ifq);
 835                         break;
 836
 837                 case PRIQ_DISABLE:
 838                         error = altq_disable(pif->pif_ifq);
 839                         break;
 840
 841                 case PRIQ_CLEAR:
 842                         priq_clear_interface(pif);
 843                         break;
 844                 }
 845                 break;
 846
 847         case PRIQ_ADD_CLASS:
 848                 error = priqcmd_add_class((struct priq_add_class *)addr);
 849                 break;
 850
 851         case PRIQ_DEL_CLASS:
 852                 error = priqcmd_delete_class((struct priq_delete_class *)addr);
 853                 break;
 854
 855         case PRIQ_MOD_CLASS:
 856                 error = priqcmd_modify_class((struct priq_modify_class *)addr);
 857                 break;
 858
 859         case PRIQ_ADD_FILTER:
 860                 error = priqcmd_add_filter((struct priq_add_filter *)addr);
 861                 break;
 862
 863         case PRIQ_DEL_FILTER:
 864                 error = priqcmd_delete_filter((struct priq_delete_filter *)addr);
 865                 break;
 866
 867         case PRIQ_GETSTATS:
 868                 error = priqcmd_class_stats((struct priq_class_stats *)addr);
 869                 break;
 870
 871         default:
 872                 error = EINVAL;
 873                 break;
 874         }
 875         return error;
 876 }
 877
 878 static int
 879 priqcmd_if_attach(ap)
 880         struct priq_interface *ap;
 881 {
 882         struct priq_if *pif;
 883         struct ifnet *ifp;
 884         int error;
 885
 886         if ((ifp = ifunit(ap->ifname)) == NULL)
 887                 return (ENXIO);
 888
 889         if ((pif = priq_attach(&ifp->if_snd, ap->arg)) == NULL)
 890                 return (ENOMEM);
 891
 892         /*
 893          * set PRIQ to this ifnet structure.
 894          */
 895         if ((error = altq_attach(&ifp->if_snd, ALTQT_PRIQ, pif,
 896                                  priq_enqueue, priq_dequeue, priq_request,
 897                                  &pif->pif_classifier, acc_classify)) != 0)
 898                 (void)priq_detach(pif);
 899
 900         return (error);
 901 }
 902
 903 static int
 904 priqcmd_if_detach(ap)
 905         struct priq_interface *ap;
 906 {
 907         struct priq_if *pif;
 908         int error;
 909
 910         if ((pif = altq_lookup(ap->ifname, ALTQT_PRIQ)) == NULL)
 911                 return (EBADF);
 912
 913         if (ALTQ_IS_ENABLED(pif->pif_ifq))
 914                 altq_disable(pif->pif_ifq);
 915
 916         if ((error = altq_detach(pif->pif_ifq)))
 917                 return (error);
 918
 919         return priq_detach(pif);
 920 }
 921
 922 static int
 923 priqcmd_add_class(ap)
 924         struct priq_add_class *ap;
 925 {
 926         struct priq_if *pif;
 927         struct priq_class *cl;
 928         int qid;
 929
 930         if ((pif = altq_lookup(ap->iface.ifname, ALTQT_PRIQ)) == NULL)
 931                 return (EBADF);
 932
 933         if (ap->pri < 0 || ap->pri >= PRIQ_MAXPRI)
 934                 return (EINVAL);
 935         if (pif->pif_classes[ap->pri] != NULL)
 936                 return (EBUSY);
 937
 938         qid = ap->pri + 1;
 939         if ((cl = priq_class_create(pif, ap->pri,
 940             ap->qlimit, ap->flags, qid)) == NULL)
 941                 return (ENOMEM);
 942
 943         /* return a class handle to the user */
 944         ap->class_handle = cl->cl_handle;
 945
 946         return (0);
 947 }
 948
 949 static int
 950 priqcmd_delete_class(ap)
 951         struct priq_delete_class *ap;
 952 {
 953         struct priq_if *pif;
 954         struct priq_class *cl;
 955
 956         if ((pif = altq_lookup(ap->iface.ifname, ALTQT_PRIQ)) == NULL)
 957                 return (EBADF);
 958
 959         if ((cl = clh_to_clp(pif, ap->class_handle)) == NULL)
 960                 return (EINVAL);
 961
 962         return priq_class_destroy(cl);
 963 }
 964
 965 static int
 966 priqcmd_modify_class(ap)
 967         struct priq_modify_class *ap;
 968 {
 969         struct priq_if *pif;
 970         struct priq_class *cl;
 971
 972         if ((pif = altq_lookup(ap->iface.ifname, ALTQT_PRIQ)) == NULL)
 973                 return (EBADF);
 974
 975         if (ap->pri < 0 || ap->pri >= PRIQ_MAXPRI)
 976                 return (EINVAL);
 977
 978         if ((cl = clh_to_clp(pif, ap->class_handle)) == NULL)
 979                 return (EINVAL);
 980
 981         /*
 982          * if priority is changed, move the class to the new priority
 983          */
 984         if (pif->pif_classes[ap->pri] != cl) {
 985                 if (pif->pif_classes[ap->pri] != NULL)
 986                         return (EEXIST);
 987                 pif->pif_classes[cl->cl_pri] = NULL;
 988                 pif->pif_classes[ap->pri] = cl;
 989                 cl->cl_pri = ap->pri;
 990         }
 991
 992         /* call priq_class_create to change class parameters */
 993         if ((cl = priq_class_create(pif, ap->pri,
 994             ap->qlimit, ap->flags, ap->class_handle)) == NULL)
 995                 return (ENOMEM);
 996         return 0;
 997 }
 998
 999 static int
1000 priqcmd_add_filter(ap)
1001         struct priq_add_filter *ap;
1002 {
1003         struct priq_if *pif;
1004         struct priq_class *cl;
1005
1006         if ((pif = altq_lookup(ap->iface.ifname, ALTQT_PRIQ)) == NULL)
1007                 return (EBADF);
1008
1009         if ((cl = clh_to_clp(pif, ap->class_handle)) == NULL)
1010                 return (EINVAL);
1011
1012         return acc_add_filter(&pif->pif_classifier, &ap->filter,
1013                               cl, &ap->filter_handle);
1014 }
1015
1016 static int
1017 priqcmd_delete_filter(ap)
1018         struct priq_delete_filter *ap;
1019 {
1020         struct priq_if *pif;
1021
1022         if ((pif = altq_lookup(ap->iface.ifname, ALTQT_PRIQ)) == NULL)
1023                 return (EBADF);
1024
1025         return acc_delete_filter(&pif->pif_classifier,
1026                                  ap->filter_handle);
1027 }
1028
1029 static int
1030 priqcmd_class_stats(ap)
1031         struct priq_class_stats *ap;
1032 {
1033         struct priq_if *pif;
1034         struct priq_class *cl;
1035         struct priq_classstats stats, *usp;
1036         int     pri, error;
1037
1038         if ((pif = altq_lookup(ap->iface.ifname, ALTQT_PRIQ)) == NULL)
1039                 return (EBADF);
1040
1041         ap->maxpri = pif->pif_maxpri;
1042
1043         /* then, read the next N classes in the tree */
1044         usp = ap->stats;
1045         for (pri = 0; pri <= pif->pif_maxpri; pri++) {
1046                 cl = pif->pif_classes[pri];
1047                 if (cl != NULL)
1048                         get_class_stats(&stats, cl);
1049                 else
1050                         bzero(&stats, sizeof(stats));
1051                 if ((error = copyout((caddr_t)&stats, (caddr_t)usp++,
1052                                      sizeof(stats))) != 0)
1053                         return (error);
1054         }
1055         return (0);
1056 }
1057
1058 #ifdef KLD_MODULE
1059
1060 static struct altqsw priq_sw =
1061         {"priq", priqopen, priqclose, priqioctl};
1062
1063 ALTQ_MODULE(altq_priq, ALTQT_PRIQ, &priq_sw);
1064 MODULE_DEPEND(altq_priq, altq_red, 1, 1, 1);
1065 MODULE_DEPEND(altq_priq, altq_rio, 1, 1, 1);
1066
1067 #endif /* KLD_MODULE */
1068
1069 #endif /* ALTQ3_COMPAT */
1070 #endif /* ALTQ_PRIQ */