1 /* $OpenBSD: altq_priq.c,v 1.17 2004/01/14 08:42:23 kjc Exp $ */
2 /* $KAME: altq_priq.c,v 1.1 2000/10/18 09:15:23 kjc Exp $ */
3 /*
4 * Copyright (C) 2000
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 #include <sys/param.h>
33 #include <sys/malloc.h>
34 #include <sys/mbuf.h>
35 #include <sys/socket.h>
36 #include <sys/systm.h>
37 #include <sys/proc.h>
38 #include <sys/errno.h>
39 #include <sys/kernel.h>
40 #include <sys/queue.h>
41
42 #include <net/if.h>
43 #include <netinet/in.h>
44
45 #include <net/pfvar.h>
46 #include <altq/altq.h>
47 #include <altq/altq_priq.h>
48
49 /*
50 * function prototypes
51 */
52 static int priq_clear_interface(struct priq_if *);
53 static int priq_request(struct ifaltq *, int, void *);
54 static void priq_purge(struct priq_if *);
55 static struct priq_class *priq_class_create(struct priq_if *, int, int, int,
56 int);
57 static int priq_class_destroy(struct priq_class *);
58 static int priq_enqueue(struct ifaltq *, struct mbuf *,
59 struct altq_pktattr *);
60 static struct mbuf *priq_dequeue(struct ifaltq *, int);
61
62 static int priq_addq(struct priq_class *, struct mbuf *);
63 static struct mbuf *priq_getq(struct priq_class *);
64 static struct mbuf *priq_pollq(struct priq_class *);
65 static void priq_purgeq(struct priq_class *);
66
67 static void get_class_stats(struct priq_classstats *, struct priq_class *);
68 static struct priq_class *clh_to_clp(struct priq_if *, u_int32_t);
69
70 int
priq_pfattach(struct pf_altq * a)71 priq_pfattach(struct pf_altq *a)
72 {
73 struct ifnet *ifp;
74 int s, error;
75
76 if ((ifp = ifunit(a->ifname)) == NULL || a->altq_disc == NULL)
77 return (EINVAL);
78 s = splimp();
79 error = altq_attach(&ifp->if_snd, ALTQT_PRIQ, a->altq_disc,
80 priq_enqueue, priq_dequeue, priq_request, NULL, NULL);
81 splx(s);
82 return (error);
83 }
84
85 int
priq_add_altq(struct pf_altq * a)86 priq_add_altq(struct pf_altq *a)
87 {
88 struct priq_if *pif;
89 struct ifnet *ifp;
90
91 if ((ifp = ifunit(a->ifname)) == NULL)
92 return (EINVAL);
93 if (!ALTQ_IS_READY(&ifp->if_snd))
94 return (ENODEV);
95
96 MALLOC(pif, struct priq_if *, sizeof(struct priq_if),
97 M_DEVBUF, M_WAITOK);
98 if (pif == NULL)
99 return (ENOMEM);
100 bzero(pif, sizeof(struct priq_if));
101 pif->pif_bandwidth = a->ifbandwidth;
102 pif->pif_maxpri = -1;
103 pif->pif_ifq = &ifp->if_snd;
104
105 /* keep the state in pf_altq */
106 a->altq_disc = pif;
107
108 return (0);
109 }
110
111 int
priq_remove_altq(struct pf_altq * a)112 priq_remove_altq(struct pf_altq *a)
113 {
114 struct priq_if *pif;
115
116 if ((pif = a->altq_disc) == NULL)
117 return (EINVAL);
118 a->altq_disc = NULL;
119
120 (void)priq_clear_interface(pif);
121
122 FREE(pif, M_DEVBUF);
123 return (0);
124 }
125
126 int
priq_add_queue(struct pf_altq * a)127 priq_add_queue(struct pf_altq *a)
128 {
129 struct priq_if *pif;
130 struct priq_class *cl;
131
132 if ((pif = a->altq_disc) == NULL)
133 return (EINVAL);
134
135 /* check parameters */
136 if (a->priority >= PRIQ_MAXPRI)
137 return (EINVAL);
138 if (a->qid == 0)
139 return (EINVAL);
140 if (pif->pif_classes[a->priority] != NULL)
141 return (EBUSY);
142 if (clh_to_clp(pif, a->qid) != NULL)
143 return (EBUSY);
144
145 cl = priq_class_create(pif, a->priority, a->qlimit,
146 a->pq_u.priq_opts.flags, a->qid);
147 if (cl == NULL)
148 return (ENOMEM);
149
150 return (0);
151 }
152
153 int
priq_remove_queue(struct pf_altq * a)154 priq_remove_queue(struct pf_altq *a)
155 {
156 struct priq_if *pif;
157 struct priq_class *cl;
158
159 if ((pif = a->altq_disc) == NULL)
160 return (EINVAL);
161
162 if ((cl = clh_to_clp(pif, a->qid)) == NULL)
163 return (EINVAL);
164
165 return (priq_class_destroy(cl));
166 }
167
168 int
priq_getqstats(struct pf_altq * a,void * ubuf,int * nbytes)169 priq_getqstats(struct pf_altq *a, void *ubuf, int *nbytes)
170 {
171 struct priq_if *pif;
172 struct priq_class *cl;
173 struct priq_classstats stats;
174 int error = 0;
175
176 if ((pif = altq_lookup(a->ifname, ALTQT_PRIQ)) == NULL)
177 return (EBADF);
178
179 if ((cl = clh_to_clp(pif, a->qid)) == NULL)
180 return (EINVAL);
181
182 if (*nbytes < sizeof(stats))
183 return (EINVAL);
184
185 get_class_stats(&stats, cl);
186
187 if ((error = copyout((caddr_t)&stats, ubuf, sizeof(stats))) != 0)
188 return (error);
189 *nbytes = sizeof(stats);
190 return (0);
191 }
192
193 /*
194 * bring the interface back to the initial state by discarding
195 * all the filters and classes.
196 */
197 static int
priq_clear_interface(struct priq_if * pif)198 priq_clear_interface(struct priq_if *pif)
199 {
200 struct priq_class *cl;
201 int pri;
202
203 /* clear out the classes */
204 for (pri = 0; pri <= pif->pif_maxpri; pri++)
205 if ((cl = pif->pif_classes[pri]) != NULL)
206 priq_class_destroy(cl);
207
208 return (0);
209 }
210
211 static int
priq_request(struct ifaltq * ifq,int req,void * arg)212 priq_request(struct ifaltq *ifq, int req, void *arg)
213 {
214 struct priq_if *pif = (struct priq_if *)ifq->altq_disc;
215
216 switch (req) {
217 case ALTRQ_PURGE:
218 priq_purge(pif);
219 break;
220 }
221 return (0);
222 }
223
224 /* discard all the queued packets on the interface */
225 static void
priq_purge(struct priq_if * pif)226 priq_purge(struct priq_if *pif)
227 {
228 struct priq_class *cl;
229 int pri;
230
231 for (pri = 0; pri <= pif->pif_maxpri; pri++) {
232 if ((cl = pif->pif_classes[pri]) != NULL && !qempty(cl->cl_q))
233 priq_purgeq(cl);
234 }
235 if (ALTQ_IS_ENABLED(pif->pif_ifq))
236 pif->pif_ifq->ifq_len = 0;
237 }
238
239 static struct priq_class *
priq_class_create(struct priq_if * pif,int pri,int qlimit,int flags,int qid)240 priq_class_create(struct priq_if *pif, int pri, int qlimit, int flags, int qid)
241 {
242 struct priq_class *cl;
243 int s;
244
245 #ifndef ALTQ_RED
246 if (flags & PRCF_RED) {
247 #ifdef ALTQ_DEBUG
248 printf("priq_class_create: RED not configured for PRIQ!\n");
249 #endif
250 return (NULL);
251 }
252 #endif
253
254 if ((cl = pif->pif_classes[pri]) != NULL) {
255 /* modify the class instead of creating a new one */
256 s = splimp();
257 if (!qempty(cl->cl_q))
258 priq_purgeq(cl);
259 splx(s);
260 #ifdef ALTQ_RIO
261 if (q_is_rio(cl->cl_q))
262 rio_destroy((rio_t *)cl->cl_red);
263 #endif
264 #ifdef ALTQ_RED
265 if (q_is_red(cl->cl_q))
266 red_destroy(cl->cl_red);
267 #endif
268 } else {
269 MALLOC(cl, struct priq_class *, sizeof(struct priq_class),
270 M_DEVBUF, M_WAITOK);
271 if (cl == NULL)
272 return (NULL);
273 bzero(cl, sizeof(struct priq_class));
274
275 MALLOC(cl->cl_q, class_queue_t *, sizeof(class_queue_t),
276 M_DEVBUF, M_WAITOK);
277 if (cl->cl_q == NULL)
278 goto err_ret;
279 bzero(cl->cl_q, sizeof(class_queue_t));
280 }
281
282 pif->pif_classes[pri] = cl;
283 if (flags & PRCF_DEFAULTCLASS)
284 pif->pif_default = cl;
285 if (qlimit == 0)
286 qlimit = 50; /* use default */
287 qlimit(cl->cl_q) = qlimit;
288 qtype(cl->cl_q) = Q_DROPTAIL;
289 qlen(cl->cl_q) = 0;
290 cl->cl_flags = flags;
291 cl->cl_pri = pri;
292 if (pri > pif->pif_maxpri)
293 pif->pif_maxpri = pri;
294 cl->cl_pif = pif;
295 cl->cl_handle = qid;
296
297 #ifdef ALTQ_RED
298 if (flags & (PRCF_RED|PRCF_RIO)) {
299 int red_flags, red_pkttime;
300
301 red_flags = 0;
302 if (flags & PRCF_ECN)
303 red_flags |= REDF_ECN;
304 #ifdef ALTQ_RIO
305 if (flags & PRCF_CLEARDSCP)
306 red_flags |= RIOF_CLEARDSCP;
307 #endif
308 if (pif->pif_bandwidth < 8)
309 red_pkttime = 1000 * 1000 * 1000; /* 1 sec */
310 else
311 red_pkttime = (int64_t)pif->pif_ifq->altq_ifp->if_mtu
312 * 1000 * 1000 * 1000 / (pif->pif_bandwidth / 8);
313 #ifdef ALTQ_RIO
314 if (flags & PRCF_RIO) {
315 cl->cl_red = (red_t *)rio_alloc(0, NULL,
316 red_flags, red_pkttime);
317 if (cl->cl_red != NULL)
318 qtype(cl->cl_q) = Q_RIO;
319 } else
320 #endif
321 if (flags & PRCF_RED) {
322 cl->cl_red = red_alloc(0, 0,
323 qlimit(cl->cl_q) * 10/100,
324 qlimit(cl->cl_q) * 30/100,
325 red_flags, red_pkttime);
326 if (cl->cl_red != NULL)
327 qtype(cl->cl_q) = Q_RED;
328 }
329 }
330 #endif /* ALTQ_RED */
331
332 return (cl);
333
334 err_ret:
335 if (cl->cl_red != NULL) {
336 #ifdef ALTQ_RIO
337 if (q_is_rio(cl->cl_q))
338 rio_destroy((rio_t *)cl->cl_red);
339 #endif
340 #ifdef ALTQ_RED
341 if (q_is_red(cl->cl_q))
342 red_destroy(cl->cl_red);
343 #endif
344 }
345 if (cl->cl_q != NULL)
346 FREE(cl->cl_q, M_DEVBUF);
347 FREE(cl, M_DEVBUF);
348 return (NULL);
349 }
350
351 static int
priq_class_destroy(struct priq_class * cl)352 priq_class_destroy(struct priq_class *cl)
353 {
354 struct priq_if *pif;
355 int s, pri;
356
357 s = splimp();
358
359 if (!qempty(cl->cl_q))
360 priq_purgeq(cl);
361
362 pif = cl->cl_pif;
363 pif->pif_classes[cl->cl_pri] = NULL;
364 if (pif->pif_maxpri == cl->cl_pri) {
365 for (pri = cl->cl_pri; pri >= 0; pri--)
366 if (pif->pif_classes[pri] != NULL) {
367 pif->pif_maxpri = pri;
368 break;
369 }
370 if (pri < 0)
371 pif->pif_maxpri = -1;
372 }
373 splx(s);
374
375 if (cl->cl_red != NULL) {
376 #ifdef ALTQ_RIO
377 if (q_is_rio(cl->cl_q))
378 rio_destroy((rio_t *)cl->cl_red);
379 #endif
380 #ifdef ALTQ_RED
381 if (q_is_red(cl->cl_q))
382 red_destroy(cl->cl_red);
383 #endif
384 }
385 FREE(cl->cl_q, M_DEVBUF);
386 FREE(cl, M_DEVBUF);
387 return (0);
388 }
389
390 /*
391 * priq_enqueue is an enqueue function to be registered to
392 * (*altq_enqueue) in struct ifaltq.
393 */
394 static int
priq_enqueue(struct ifaltq * ifq,struct mbuf * m,struct altq_pktattr * pktattr)395 priq_enqueue(struct ifaltq *ifq, struct mbuf *m, struct altq_pktattr *pktattr)
396 {
397 struct priq_if *pif = (struct priq_if *)ifq->altq_disc;
398 struct priq_class *cl;
399 struct m_tag *t;
400 int len;
401
402 /* grab class set by classifier */
403 if ((m->m_flags & M_PKTHDR) == 0) {
404 /* should not happen */
405 printf("altq: packet for %s does not have pkthdr\n",
406 ifq->altq_ifp->if_xname);
407 m_freem(m);
408 return (ENOBUFS);
409 }
410 t = m_tag_find(m, PACKET_TAG_PF_QID, NULL);
411 if (t == NULL ||
412 (cl = clh_to_clp(pif, ((struct altq_tag *)(t+1))->qid)) == NULL) {
413 cl = pif->pif_default;
414 if (cl == NULL) {
415 m_freem(m);
416 return (ENOBUFS);
417 }
418 cl->cl_pktattr = NULL;
419 }
420
421 len = m_pktlen(m);
422 if (priq_addq(cl, m) != 0) {
423 /* drop occurred. mbuf was freed in priq_addq. */
424 PKTCNTR_ADD(&cl->cl_dropcnt, len);
425 return (ENOBUFS);
426 }
427 IFQ_INC_LEN(ifq);
428
429 /* successfully queued. */
430 return (0);
431 }
432
433 /*
434 * priq_dequeue is a dequeue function to be registered to
435 * (*altq_dequeue) in struct ifaltq.
436 *
437 * note: ALTDQ_POLL returns the next packet without removing the packet
438 * from the queue. ALTDQ_REMOVE is a normal dequeue operation.
439 * ALTDQ_REMOVE must return the same packet if called immediately
440 * after ALTDQ_POLL.
441 */
442 static struct mbuf *
priq_dequeue(struct ifaltq * ifq,int op)443 priq_dequeue(struct ifaltq *ifq, int op)
444 {
445 struct priq_if *pif = (struct priq_if *)ifq->altq_disc;
446 struct priq_class *cl;
447 struct mbuf *m;
448 int pri;
449
450 if (IFQ_IS_EMPTY(ifq))
451 /* no packet in the queue */
452 return (NULL);
453
454 for (pri = pif->pif_maxpri; pri >= 0; pri--) {
455 if ((cl = pif->pif_classes[pri]) != NULL &&
456 !qempty(cl->cl_q)) {
457 if (op == ALTDQ_POLL)
458 return (priq_pollq(cl));
459
460 m = priq_getq(cl);
461 if (m != NULL) {
462 IFQ_DEC_LEN(ifq);
463 if (qempty(cl->cl_q))
464 cl->cl_period++;
465 PKTCNTR_ADD(&cl->cl_xmitcnt, m_pktlen(m));
466 }
467 return (m);
468 }
469 }
470 return (NULL);
471 }
472
473 static int
priq_addq(struct priq_class * cl,struct mbuf * m)474 priq_addq(struct priq_class *cl, struct mbuf *m)
475 {
476
477 #ifdef ALTQ_RIO
478 if (q_is_rio(cl->cl_q))
479 return rio_addq((rio_t *)cl->cl_red, cl->cl_q, m,
480 cl->cl_pktattr);
481 #endif
482 #ifdef ALTQ_RED
483 if (q_is_red(cl->cl_q))
484 return red_addq(cl->cl_red, cl->cl_q, m, cl->cl_pktattr);
485 #endif
486 if (qlen(cl->cl_q) >= qlimit(cl->cl_q)) {
487 m_freem(m);
488 return (-1);
489 }
490
491 if (cl->cl_flags & PRCF_CLEARDSCP)
492 write_dsfield(m, cl->cl_pktattr, 0);
493
494 _addq(cl->cl_q, m);
495
496 return (0);
497 }
498
499 static struct mbuf *
priq_getq(struct priq_class * cl)500 priq_getq(struct priq_class *cl)
501 {
502 #ifdef ALTQ_RIO
503 if (q_is_rio(cl->cl_q))
504 return rio_getq((rio_t *)cl->cl_red, cl->cl_q);
505 #endif
506 #ifdef ALTQ_RED
507 if (q_is_red(cl->cl_q))
508 return red_getq(cl->cl_red, cl->cl_q);
509 #endif
510 return _getq(cl->cl_q);
511 }
512
513 static struct mbuf *
priq_pollq(cl)514 priq_pollq(cl)
515 struct priq_class *cl;
516 {
517 return qhead(cl->cl_q);
518 }
519
520 static void
priq_purgeq(struct priq_class * cl)521 priq_purgeq(struct priq_class *cl)
522 {
523 struct mbuf *m;
524
525 if (qempty(cl->cl_q))
526 return;
527
528 while ((m = _getq(cl->cl_q)) != NULL) {
529 PKTCNTR_ADD(&cl->cl_dropcnt, m_pktlen(m));
530 m_freem(m);
531 }
532 ASSERT(qlen(cl->cl_q) == 0);
533 }
534
535 static void
get_class_stats(struct priq_classstats * sp,struct priq_class * cl)536 get_class_stats(struct priq_classstats *sp, struct priq_class *cl)
537 {
538 sp->class_handle = cl->cl_handle;
539 sp->qlength = qlen(cl->cl_q);
540 sp->qlimit = qlimit(cl->cl_q);
541 sp->period = cl->cl_period;
542 sp->xmitcnt = cl->cl_xmitcnt;
543 sp->dropcnt = cl->cl_dropcnt;
544
545 sp->qtype = qtype(cl->cl_q);
546 #ifdef ALTQ_RED
547 if (q_is_red(cl->cl_q))
548 red_getstats(cl->cl_red, &sp->red[0]);
549 #endif
550 #ifdef ALTQ_RIO
551 if (q_is_rio(cl->cl_q))
552 rio_getstats((rio_t *)cl->cl_red, &sp->red[0]);
553 #endif
554
555 }
556
557 /* convert a class handle to the corresponding class pointer */
558 static struct priq_class *
clh_to_clp(struct priq_if * pif,u_int32_t chandle)559 clh_to_clp(struct priq_if *pif, u_int32_t chandle)
560 {
561 struct priq_class *cl;
562 int idx;
563
564 if (chandle == 0)
565 return (NULL);
566
567 for (idx = pif->pif_maxpri; idx >= 0; idx--)
568 if ((cl = pif->pif_classes[idx]) != NULL &&
569 cl->cl_handle == chandle)
570 return (cl);
571
572 return (NULL);
573 }
574