xref: /dragonfly/sys/net/altq/altq_priq.c (revision 9ddb8543) 
1 /*	$KAME: altq_priq.c,v 1.12 2004/04/17 10:54:48 kjc Exp $	*/
2 /*	$DragonFly: src/sys/net/altq/altq_priq.c,v 1.9 2008/05/14 11:59:23 sephe Exp $ */
3 
4 /*
5  * Copyright (C) 2000-2003
6  *	Sony Computer Science Laboratories Inc.  All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  * 1. Redistributions of source code must retain the above copyright
12  *    notice, this list of conditions and the following disclaimer.
13  * 2. Redistributions in binary form must reproduce the above copyright
14  *    notice, this list of conditions and the following disclaimer in the
15  *    documentation and/or other materials provided with the distribution.
16  *
17  * THIS SOFTWARE IS PROVIDED BY SONY CSL AND CONTRIBUTORS ``AS IS'' AND
18  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED.  IN NO EVENT SHALL SONY CSL OR CONTRIBUTORS BE LIABLE
21  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
23  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
24  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
25  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
26  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
27  * SUCH DAMAGE.
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 #include <sys/thread.h>
50 
51 #include <net/if.h>
52 #include <net/ifq_var.h>
53 #include <netinet/in.h>
54 
55 #include <net/pf/pfvar.h>
56 #include <net/altq/altq.h>
57 #include <net/altq/altq_priq.h>
58 
59 #include <sys/thread2.h>
60 
61 /*
62  * function prototypes
63  */
64 static int	priq_clear_interface(struct priq_if *);
65 static int	priq_request(struct ifaltq *, int, void *);
66 static void	priq_purge(struct priq_if *);
67 static struct priq_class *priq_class_create(struct priq_if *, int, int, int, int);
68 static int	priq_class_destroy(struct priq_class *);
69 static int	priq_enqueue(struct ifaltq *, struct mbuf *, struct altq_pktattr *);
70 static struct mbuf *priq_dequeue(struct ifaltq *, struct mbuf *, int);
71 
72 static int	priq_addq(struct priq_class *, struct mbuf *);
73 static struct mbuf *priq_getq(struct priq_class *);
74 static struct mbuf *priq_pollq(struct priq_class *);
75 static void	priq_purgeq(struct priq_class *);
76 
77 static void	get_class_stats(struct priq_classstats *, struct priq_class *);
78 static struct priq_class *clh_to_clp(struct priq_if *, uint32_t);
79 
80 int
81 priq_pfattach(struct pf_altq *a, struct ifaltq *ifq)
82 {
83 	return altq_attach(ifq, ALTQT_PRIQ, a->altq_disc,
84 	    priq_enqueue, priq_dequeue, priq_request, NULL, NULL);
85 }
86 
87 int
88 priq_add_altq(struct pf_altq *a)
89 {
90 	struct priq_if *pif;
91 	struct ifnet *ifp;
92 
93 	if ((ifp = ifunit(a->ifname)) == NULL)
94 		return (EINVAL);
95 	if (!ifq_is_ready(&ifp->if_snd))
96 		return (ENODEV);
97 
98 	pif = kmalloc(sizeof(*pif), M_ALTQ, M_WAITOK | M_ZERO);
99 	pif->pif_bandwidth = a->ifbandwidth;
100 	pif->pif_maxpri = -1;
101 	pif->pif_ifq = &ifp->if_snd;
102 	ifq_purge(&ifp->if_snd);
103 
104 	/* keep the state in pf_altq */
105 	a->altq_disc = pif;
106 
107 	return (0);
108 }
109 
110 int
111 priq_remove_altq(struct pf_altq *a)
112 {
113 	struct priq_if *pif;
114 
115 	if ((pif = a->altq_disc) == NULL)
116 		return (EINVAL);
117 	a->altq_disc = NULL;
118 
119 	priq_clear_interface(pif);
120 
121 	kfree(pif, M_ALTQ);
122 	return (0);
123 }
124 
125 static int
126 priq_add_queue_locked(struct pf_altq *a, struct priq_if *pif)
127 {
128 	struct priq_class *cl;
129 
130 	KKASSERT(a->priority < PRIQ_MAXPRI);
131 	KKASSERT(a->qid != 0);
132 
133 	if (pif->pif_classes[a->priority] != NULL)
134 		return (EBUSY);
135 	if (clh_to_clp(pif, a->qid) != NULL)
136 		return (EBUSY);
137 
138 	cl = priq_class_create(pif, a->priority, a->qlimit,
139 			       a->pq_u.priq_opts.flags, a->qid);
140 	if (cl == NULL)
141 		return (ENOMEM);
142 
143 	return (0);
144 }
145 
146 int
147 priq_add_queue(struct pf_altq *a)
148 {
149 	struct priq_if *pif;
150 	struct ifaltq *ifq;
151 	int error;
152 
153 	/* check parameters */
154 	if (a->priority >= PRIQ_MAXPRI)
155 		return (EINVAL);
156 	if (a->qid == 0)
157 		return (EINVAL);
158 
159 	/* XXX not MP safe */
160 	if ((pif = a->altq_disc) == NULL)
161 		return (EINVAL);
162 	ifq = pif->pif_ifq;
163 
164 	ALTQ_LOCK(ifq);
165 	error = priq_add_queue_locked(a, pif);
166 	ALTQ_UNLOCK(ifq);
167 
168 	return error;
169 }
170 
171 static int
172 priq_remove_queue_locked(struct pf_altq *a, struct priq_if *pif)
173 {
174 	struct priq_class *cl;
175 
176 	if ((cl = clh_to_clp(pif, a->qid)) == NULL)
177 		return (EINVAL);
178 
179 	return (priq_class_destroy(cl));
180 }
181 
182 int
183 priq_remove_queue(struct pf_altq *a)
184 {
185 	struct priq_if *pif;
186 	struct ifaltq *ifq;
187 	int error;
188 
189 	/* XXX not MF safe */
190 	if ((pif = a->altq_disc) == NULL)
191 		return (EINVAL);
192 	ifq = pif->pif_ifq;
193 
194 	ALTQ_LOCK(ifq);
195 	error = priq_remove_queue_locked(a, pif);
196 	ALTQ_UNLOCK(ifq);
197 
198 	return error;
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 	struct ifaltq *ifq;
208 	int error = 0;
209 
210 	if (*nbytes < sizeof(stats))
211 		return (EINVAL);
212 
213 	/* XXX not MP safe */
214 	if ((pif = altq_lookup(a->ifname, ALTQT_PRIQ)) == NULL)
215 		return (EBADF);
216 	ifq = pif->pif_ifq;
217 
218 	ALTQ_LOCK(ifq);
219 
220 	if ((cl = clh_to_clp(pif, a->qid)) == NULL) {
221 		ALTQ_UNLOCK(ifq);
222 		return (EINVAL);
223 	}
224 
225 	get_class_stats(&stats, cl);
226 
227 	ALTQ_UNLOCK(ifq);
228 
229 	if ((error = copyout((caddr_t)&stats, ubuf, sizeof(stats))) != 0)
230 		return (error);
231 	*nbytes = sizeof(stats);
232 	return (0);
233 }
234 
235 /*
236  * bring the interface back to the initial state by discarding
237  * all the filters and classes.
238  */
239 static int
240 priq_clear_interface(struct priq_if *pif)
241 {
242 	struct priq_class *cl;
243 	int pri;
244 
245 	/* clear out the classes */
246 	for (pri = 0; pri <= pif->pif_maxpri; pri++) {
247 		if ((cl = pif->pif_classes[pri]) != NULL)
248 			priq_class_destroy(cl);
249 	}
250 
251 	return (0);
252 }
253 
254 static int
255 priq_request(struct ifaltq *ifq, int req, void *arg)
256 {
257 	struct priq_if *pif = (struct priq_if *)ifq->altq_disc;
258 
259 	crit_enter();
260 	switch (req) {
261 	case ALTRQ_PURGE:
262 		priq_purge(pif);
263 		break;
264 	}
265 	crit_exit();
266 	return (0);
267 }
268 
269 /* discard all the queued packets on the interface */
270 static void
271 priq_purge(struct priq_if *pif)
272 {
273 	struct priq_class *cl;
274 	int pri;
275 
276 	for (pri = 0; pri <= pif->pif_maxpri; pri++) {
277 		if ((cl = pif->pif_classes[pri]) != NULL && !qempty(cl->cl_q))
278 			priq_purgeq(cl);
279 	}
280 	if (ifq_is_enabled(pif->pif_ifq))
281 		pif->pif_ifq->ifq_len = 0;
282 }
283 
284 static struct priq_class *
285 priq_class_create(struct priq_if *pif, int pri, int qlimit, int flags, int qid)
286 {
287 	struct priq_class *cl;
288 
289 #ifndef ALTQ_RED
290 	if (flags & PRCF_RED) {
291 #ifdef ALTQ_DEBUG
292 		kprintf("priq_class_create: RED not configured for PRIQ!\n");
293 #endif
294 		return (NULL);
295 	}
296 #endif
297 
298 	if ((cl = pif->pif_classes[pri]) != NULL) {
299 		/* modify the class instead of creating a new one */
300 		crit_enter();
301 		if (!qempty(cl->cl_q))
302 			priq_purgeq(cl);
303 		crit_exit();
304 #ifdef ALTQ_RIO
305 		if (q_is_rio(cl->cl_q))
306 			rio_destroy((rio_t *)cl->cl_red);
307 #endif
308 #ifdef ALTQ_RED
309 		if (q_is_red(cl->cl_q))
310 			red_destroy(cl->cl_red);
311 #endif
312 	} else {
313 		cl = kmalloc(sizeof(*cl), M_ALTQ, M_WAITOK | M_ZERO);
314 		cl->cl_q = kmalloc(sizeof(*cl->cl_q), M_ALTQ, M_WAITOK | M_ZERO);
315 	}
316 
317 	pif->pif_classes[pri] = cl;
318 	if (flags & PRCF_DEFAULTCLASS)
319 		pif->pif_default = cl;
320 	if (qlimit == 0)
321 		qlimit = 50;  /* use default */
322 	qlimit(cl->cl_q) = qlimit;
323 	qtype(cl->cl_q) = Q_DROPTAIL;
324 	qlen(cl->cl_q) = 0;
325 	cl->cl_flags = flags;
326 	cl->cl_pri = pri;
327 	if (pri > pif->pif_maxpri)
328 		pif->pif_maxpri = pri;
329 	cl->cl_pif = pif;
330 	cl->cl_handle = qid;
331 
332 #ifdef ALTQ_RED
333 	if (flags & (PRCF_RED|PRCF_RIO)) {
334 		int red_flags, red_pkttime;
335 
336 		red_flags = 0;
337 		if (flags & PRCF_ECN)
338 			red_flags |= REDF_ECN;
339 #ifdef ALTQ_RIO
340 		if (flags & PRCF_CLEARDSCP)
341 			red_flags |= RIOF_CLEARDSCP;
342 #endif
343 		if (pif->pif_bandwidth < 8)
344 			red_pkttime = 1000 * 1000 * 1000; /* 1 sec */
345 		else
346 			red_pkttime = (int64_t)pif->pif_ifq->altq_ifp->if_mtu
347 			  * 1000 * 1000 * 1000 / (pif->pif_bandwidth / 8);
348 #ifdef ALTQ_RIO
349 		if (flags & PRCF_RIO) {
350 			cl->cl_red = (red_t *)rio_alloc(0, NULL,
351 						red_flags, red_pkttime);
352 			if (cl->cl_red != NULL)
353 				qtype(cl->cl_q) = Q_RIO;
354 		} else
355 #endif
356 		if (flags & PRCF_RED) {
357 			cl->cl_red = red_alloc(0, 0,
358 			    qlimit(cl->cl_q) * 10/100,
359 			    qlimit(cl->cl_q) * 30/100,
360 			    red_flags, red_pkttime);
361 			if (cl->cl_red != NULL)
362 				qtype(cl->cl_q) = Q_RED;
363 		}
364 	}
365 #endif /* ALTQ_RED */
366 
367 	return (cl);
368 }
369 
370 static int
371 priq_class_destroy(struct priq_class *cl)
372 {
373 	struct priq_if *pif;
374 	int pri;
375 
376 	crit_enter();
377 
378 	if (!qempty(cl->cl_q))
379 		priq_purgeq(cl);
380 
381 	pif = cl->cl_pif;
382 	pif->pif_classes[cl->cl_pri] = NULL;
383 	if (pif->pif_maxpri == cl->cl_pri) {
384 		for (pri = cl->cl_pri; pri >= 0; pri--)
385 			if (pif->pif_classes[pri] != NULL) {
386 				pif->pif_maxpri = pri;
387 				break;
388 			}
389 		if (pri < 0)
390 			pif->pif_maxpri = -1;
391 	}
392 	crit_exit();
393 
394 	if (cl->cl_red != NULL) {
395 #ifdef ALTQ_RIO
396 		if (q_is_rio(cl->cl_q))
397 			rio_destroy((rio_t *)cl->cl_red);
398 #endif
399 #ifdef ALTQ_RED
400 		if (q_is_red(cl->cl_q))
401 			red_destroy(cl->cl_red);
402 #endif
403 	}
404 	kfree(cl->cl_q, M_ALTQ);
405 	kfree(cl, M_ALTQ);
406 	return (0);
407 }
408 
409 /*
410  * priq_enqueue is an enqueue function to be registered to
411  * (*altq_enqueue) in struct ifaltq.
412  */
413 static int
414 priq_enqueue(struct ifaltq *ifq, struct mbuf *m, struct altq_pktattr *pktattr)
415 {
416 	struct priq_if *pif = (struct priq_if *)ifq->altq_disc;
417 	struct priq_class *cl;
418 	int error;
419 	int len;
420 
421 	crit_enter();
422 
423 	/* grab class set by classifier */
424 	if ((m->m_flags & M_PKTHDR) == 0) {
425 		/* should not happen */
426 		if_printf(ifq->altq_ifp, "altq: packet does not have pkthdr\n");
427 		m_freem(m);
428 		error = ENOBUFS;
429 		goto done;
430 	}
431 
432 	if (m->m_pkthdr.fw_flags & ALTQ_MBUF_TAGGED)
433 		cl = clh_to_clp(pif, m->m_pkthdr.altq_qid);
434 	else
435 		cl = NULL;
436 	if (cl == NULL) {
437 		cl = pif->pif_default;
438 		if (cl == NULL) {
439 			m_freem(m);
440 			error = ENOBUFS;
441 			goto done;
442 		}
443 	}
444 	cl->cl_pktattr = NULL;
445 	len = m_pktlen(m);
446 	if (priq_addq(cl, m) != 0) {
447 		/* drop occurred.  mbuf was freed in priq_addq. */
448 		PKTCNTR_ADD(&cl->cl_dropcnt, len);
449 		error = ENOBUFS;
450 		goto done;
451 	}
452 	ifq->ifq_len++;
453 	error = 0;
454 done:
455 	crit_exit();
456 	return (error);
457 }
458 
459 /*
460  * priq_dequeue is a dequeue function to be registered to
461  * (*altq_dequeue) in struct ifaltq.
462  *
463  * note: ALTDQ_POLL returns the next packet without removing the packet
464  *	from the queue.  ALTDQ_REMOVE is a normal dequeue operation.
465  *	ALTDQ_REMOVE must return the same packet if called immediately
466  *	after ALTDQ_POLL.
467  */
468 static struct mbuf *
469 priq_dequeue(struct ifaltq *ifq, struct mbuf *mpolled, int op)
470 {
471 	struct priq_if *pif = (struct priq_if *)ifq->altq_disc;
472 	struct priq_class *cl;
473 	struct mbuf *m;
474 	int pri;
475 
476 	if (ifq_is_empty(ifq)) {
477 		/* no packet in the queue */
478 		KKASSERT(mpolled == NULL);
479 		return (NULL);
480 	}
481 
482 	crit_enter();
483 	m = NULL;
484 	for (pri = pif->pif_maxpri;  pri >= 0; pri--) {
485 		if ((cl = pif->pif_classes[pri]) != NULL && !qempty(cl->cl_q)) {
486 			if (op == ALTDQ_POLL) {
487 				m = priq_pollq(cl);
488 				break;
489 			}
490 
491 			m = priq_getq(cl);
492 			if (m != NULL) {
493 				ifq->ifq_len--;
494 				if (qempty(cl->cl_q))
495 					cl->cl_period++;
496 				PKTCNTR_ADD(&cl->cl_xmitcnt, m_pktlen(m));
497 			}
498 			break;
499 		}
500 	}
501 	crit_exit();
502 	KKASSERT(mpolled == NULL || mpolled == m);
503 	return (m);
504 }
505 
506 static int
507 priq_addq(struct priq_class *cl, struct mbuf *m)
508 {
509 #ifdef ALTQ_RIO
510 	if (q_is_rio(cl->cl_q))
511 		return rio_addq((rio_t *)cl->cl_red, cl->cl_q, m,
512 				cl->cl_pktattr);
513 #endif
514 #ifdef ALTQ_RED
515 	if (q_is_red(cl->cl_q))
516 		return red_addq(cl->cl_red, cl->cl_q, m, cl->cl_pktattr);
517 #endif
518 	if (qlen(cl->cl_q) >= qlimit(cl->cl_q)) {
519 		m_freem(m);
520 		return (-1);
521 	}
522 
523 	if (cl->cl_flags & PRCF_CLEARDSCP)
524 		write_dsfield(m, cl->cl_pktattr, 0);
525 
526 	_addq(cl->cl_q, m);
527 
528 	return (0);
529 }
530 
531 static struct mbuf *
532 priq_getq(struct priq_class *cl)
533 {
534 #ifdef ALTQ_RIO
535 	if (q_is_rio(cl->cl_q))
536 		return rio_getq((rio_t *)cl->cl_red, cl->cl_q);
537 #endif
538 #ifdef ALTQ_RED
539 	if (q_is_red(cl->cl_q))
540 		return red_getq(cl->cl_red, cl->cl_q);
541 #endif
542 	return _getq(cl->cl_q);
543 }
544 
545 static struct mbuf *
546 priq_pollq(struct priq_class *cl)
547 {
548 	return qhead(cl->cl_q);
549 }
550 
551 static void
552 priq_purgeq(struct priq_class *cl)
553 {
554 	struct mbuf *m;
555 
556 	if (qempty(cl->cl_q))
557 		return;
558 
559 	while ((m = _getq(cl->cl_q)) != NULL) {
560 		PKTCNTR_ADD(&cl->cl_dropcnt, m_pktlen(m));
561 		m_freem(m);
562 	}
563 	KKASSERT(qlen(cl->cl_q) == 0);
564 }
565 
566 static void
567 get_class_stats(struct priq_classstats *sp, struct priq_class *cl)
568 {
569 	sp->class_handle = cl->cl_handle;
570 	sp->qlength = qlen(cl->cl_q);
571 	sp->qlimit = qlimit(cl->cl_q);
572 	sp->period = cl->cl_period;
573 	sp->xmitcnt = cl->cl_xmitcnt;
574 	sp->dropcnt = cl->cl_dropcnt;
575 
576 	sp->qtype = qtype(cl->cl_q);
577 #ifdef ALTQ_RED
578 	if (q_is_red(cl->cl_q))
579 		red_getstats(cl->cl_red, &sp->red[0]);
580 #endif
581 #ifdef ALTQ_RIO
582 	if (q_is_rio(cl->cl_q))
583 		rio_getstats((rio_t *)cl->cl_red, &sp->red[0]);
584 #endif
585 }
586 
587 /* convert a class handle to the corresponding class pointer */
588 static struct priq_class *
589 clh_to_clp(struct priq_if *pif, uint32_t chandle)
590 {
591 	struct priq_class *cl;
592 	int idx;
593 
594 	if (chandle == 0)
595 		return (NULL);
596 
597 	for (idx = pif->pif_maxpri; idx >= 0; idx--)
598 		if ((cl = pif->pif_classes[idx]) != NULL &&
599 		    cl->cl_handle == chandle)
600 			return (cl);
601 
602 	return (NULL);
603 }
604 
605 #endif /* ALTQ_PRIQ */
606