sys/contrib/altq/altq/altq_priq.c

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