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
67static struct priq_if *priq_attach(struct ifaltq *, u_int);
68static int priq_detach(struct priq_if *);
69#endif
70static int priq_clear_interface(struct priq_if *);
71static int priq_request(struct ifaltq *, int, void *);
72static void priq_purge(struct priq_if *);
73static struct priq_class *priq_class_create(struct priq_if *, int, int, int,
74    int);
75static int priq_class_destroy(struct priq_class *);
76static int priq_enqueue(struct ifaltq *, struct mbuf *, struct altq_pktattr *);
77static struct mbuf *priq_dequeue(struct ifaltq *, int);
78
79static int priq_addq(struct priq_class *, struct mbuf *);
80static struct mbuf *priq_getq(struct priq_class *);
81static struct mbuf *priq_pollq(struct priq_class *);
82static void priq_purgeq(struct priq_class *);
83
84#ifdef ALTQ3_COMPAT
85static int priqcmd_if_attach(struct priq_interface *);
86static int priqcmd_if_detach(struct priq_interface *);
87static int priqcmd_add_class(struct priq_add_class *);
88static int priqcmd_delete_class(struct priq_delete_class *);
89static int priqcmd_modify_class(struct priq_modify_class *);
90static int priqcmd_add_filter(struct priq_add_filter *);
91static int priqcmd_delete_filter(struct priq_delete_filter *);
92static int priqcmd_class_stats(struct priq_class_stats *);
93#endif /* ALTQ3_COMPAT */
94
95static void get_class_stats(struct priq_classstats *, struct priq_class *);
96static struct priq_class *clh_to_clp(struct priq_if *, u_int32_t);
97
98#ifdef ALTQ3_COMPAT
99altqdev_decl(priq);
100
101/* pif_list keeps all priq_if's allocated. */
102static struct priq_if *pif_list = NULL;
103#endif /* ALTQ3_COMPAT */
104
105int
106priq_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
124int
125priq_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
150int
151priq_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
165int
166priq_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
192int
193priq_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
207int
208priq_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 */
236static int
237priq_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
255static int
256priq_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 */
271static void
272priq_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
285static struct priq_class *
286priq_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
403static int
404priq_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 */
457static int
458priq_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 */
516static struct mbuf *
517priq_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
549static int
550priq_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
575static struct mbuf *
576priq_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
589static struct mbuf *
590priq_pollq(cl)
591	struct priq_class *cl;
592{
593	return qhead(cl->cl_q);
594}
595
596static void
597priq_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
611static void
612get_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 */
634static struct priq_class *
635clh_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
654static struct priq_if *
655priq_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
677static int
678priq_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 */
704int
705priqopen(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
718int
719priqclose(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
746int
747priqioctl(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
853static int
854priqcmd_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
878static int
879priqcmd_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
897static int
898priqcmd_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
924static int
925priqcmd_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
940static int
941priqcmd_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
974static int
975priqcmd_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
991static int
992priqcmd_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
1004static int
1005priqcmd_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
1035static struct altqsw priq_sw =
1036	{"priq", priqopen, priqclose, priqioctl};
1037
1038ALTQ_MODULE(altq_priq, ALTQT_PRIQ, &priq_sw);
1039MODULE_DEPEND(altq_priq, altq_red, 1, 1, 1);
1040MODULE_DEPEND(altq_priq, altq_rio, 1, 1, 1);
1041
1042#endif /* KLD_MODULE */
1043
1044#endif /* ALTQ3_COMPAT */
1045#endif /* ALTQ_PRIQ */
1046