1 /* $OpenBSD: altq_hfsc.c,v 1.21 2004/01/14 08:42:23 kjc Exp $ */
2 /* $KAME: altq_hfsc.c,v 1.17 2002/11/29 07:48:33 kjc Exp $ */
3
4 /*
5 * Copyright (c) 1997-1999 Carnegie Mellon University. All Rights Reserved.
6 *
7 * Permission to use, copy, modify, and distribute this software and
8 * its documentation is hereby granted (including for commercial or
9 * for-profit use), provided that both the copyright notice and this
10 * permission notice appear in all copies of the software, derivative
11 * works, or modified versions, and any portions thereof.
12 *
13 * THIS SOFTWARE IS EXPERIMENTAL AND IS KNOWN TO HAVE BUGS, SOME OF
14 * WHICH MAY HAVE SERIOUS CONSEQUENCES. CARNEGIE MELLON PROVIDES THIS
15 * SOFTWARE IN ITS ``AS IS'' CONDITION, AND ANY EXPRESS OR IMPLIED
16 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
17 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 * DISCLAIMED. IN NO EVENT SHALL CARNEGIE MELLON UNIVERSITY BE LIABLE
19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
21 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
22 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
25 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26 * DAMAGE.
27 *
28 * Carnegie Mellon encourages (but does not require) users of this
29 * software to return any improvements or extensions that they make,
30 * and to grant Carnegie Mellon the rights to redistribute these
31 * changes without encumbrance.
32 */
33 /*
34 * H-FSC is described in Proceedings of SIGCOMM'97,
35 * "A Hierarchical Fair Service Curve Algorithm for Link-Sharing,
36 * Real-Time and Priority Service"
37 * by Ion Stoica, Hui Zhang, and T. S. Eugene Ng.
38 *
39 * Oleg Cherevko <olwi@aq.ml.com.ua> added the upperlimit for link-sharing.
40 * when a class has an upperlimit, the fit-time is computed from the
41 * upperlimit service curve. the link-sharing scheduler does not schedule
42 * a class whose fit-time exceeds the current time.
43 */
44
45 #include <sys/param.h>
46 #include <sys/malloc.h>
47 #include <sys/mbuf.h>
48 #include <sys/socket.h>
49 #include <sys/systm.h>
50 #include <sys/errno.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 #include <altq/altq_hfsc.h>
59
60 /*
61 * function prototypes
62 */
63 static int hfsc_clear_interface(struct hfsc_if *);
64 static int hfsc_request(struct ifaltq *, int, void *);
65 static void hfsc_purge(struct hfsc_if *);
66 static struct hfsc_class *hfsc_class_create(struct hfsc_if *,
67 struct service_curve *, struct service_curve *, struct service_curve *,
68 struct hfsc_class *, int, int, int);
69 static int hfsc_class_destroy(struct hfsc_class *);
70 static struct hfsc_class *hfsc_nextclass(struct hfsc_class *);
71 static int hfsc_enqueue(struct ifaltq *, struct mbuf *,
72 struct altq_pktattr *);
73 static struct mbuf *hfsc_dequeue(struct ifaltq *, int);
74
75 static int hfsc_addq(struct hfsc_class *, struct mbuf *);
76 static struct mbuf *hfsc_getq(struct hfsc_class *);
77 static struct mbuf *hfsc_pollq(struct hfsc_class *);
78 static void hfsc_purgeq(struct hfsc_class *);
79
80 static void update_cfmin(struct hfsc_class *);
81 static void set_active(struct hfsc_class *, int);
82 static void set_passive(struct hfsc_class *);
83
84 static void init_ed(struct hfsc_class *, int);
85 static void update_ed(struct hfsc_class *, int);
86 static void update_d(struct hfsc_class *, int);
87 static void init_vf(struct hfsc_class *, int);
88 static void update_vf(struct hfsc_class *, int, u_int64_t);
89 static ellist_t *ellist_alloc(void);
90 static void ellist_destroy(ellist_t *);
91 static void ellist_insert(struct hfsc_class *);
92 static void ellist_remove(struct hfsc_class *);
93 static void ellist_update(struct hfsc_class *);
94 struct hfsc_class *ellist_get_mindl(ellist_t *, u_int64_t);
95 static actlist_t *actlist_alloc(void);
96 static void actlist_destroy(actlist_t *);
97 static void actlist_insert(struct hfsc_class *);
98 static void actlist_remove(struct hfsc_class *);
99 static void actlist_update(struct hfsc_class *);
100
101 static struct hfsc_class *actlist_firstfit(struct hfsc_class *,
102 u_int64_t);
103
104 static __inline u_int64_t seg_x2y(u_int64_t, u_int64_t);
105 static __inline u_int64_t seg_y2x(u_int64_t, u_int64_t);
106 static __inline u_int64_t m2sm(u_int);
107 static __inline u_int64_t m2ism(u_int);
108 static __inline u_int64_t d2dx(u_int);
109 static u_int sm2m(u_int64_t);
110 static u_int dx2d(u_int64_t);
111
112 static void sc2isc(struct service_curve *, struct internal_sc *);
113 static void rtsc_init(struct runtime_sc *, struct internal_sc *,
114 u_int64_t, u_int64_t);
115 static u_int64_t rtsc_y2x(struct runtime_sc *, u_int64_t);
116 static u_int64_t rtsc_x2y(struct runtime_sc *, u_int64_t);
117 static void rtsc_min(struct runtime_sc *, struct internal_sc *,
118 u_int64_t, u_int64_t);
119
120 static void get_class_stats(struct hfsc_classstats *,
121 struct hfsc_class *);
122 static struct hfsc_class *clh_to_clp(struct hfsc_if *, u_int32_t);
123
124 /*
125 * macros
126 */
127 #define is_a_parent_class(cl) ((cl)->cl_children != NULL)
128
129 #define HT_INFINITY 0xffffffffffffffffLL /* infinite time value */
130
131 int
hfsc_pfattach(struct pf_altq * a)132 hfsc_pfattach(struct pf_altq *a)
133 {
134 struct ifnet *ifp;
135 int s, error;
136
137 if ((ifp = ifunit(a->ifname)) == NULL || a->altq_disc == NULL)
138 return (EINVAL);
139 s = splimp();
140 error = altq_attach(&ifp->if_snd, ALTQT_HFSC, a->altq_disc,
141 hfsc_enqueue, hfsc_dequeue, hfsc_request, NULL, NULL);
142 splx(s);
143 return (error);
144 }
145
146 int
hfsc_add_altq(struct pf_altq * a)147 hfsc_add_altq(struct pf_altq *a)
148 {
149 struct hfsc_if *hif;
150 struct ifnet *ifp;
151
152 if ((ifp = ifunit(a->ifname)) == NULL)
153 return (EINVAL);
154 if (!ALTQ_IS_READY(&ifp->if_snd))
155 return (ENODEV);
156
157 MALLOC(hif, struct hfsc_if *, sizeof(struct hfsc_if),
158 M_DEVBUF, M_WAITOK);
159 if (hif == NULL)
160 return (ENOMEM);
161 bzero(hif, sizeof(struct hfsc_if));
162
163 hif->hif_eligible = ellist_alloc();
164 if (hif->hif_eligible == NULL) {
165 FREE(hif, M_DEVBUF);
166 return (ENOMEM);
167 }
168
169 hif->hif_ifq = &ifp->if_snd;
170
171 /* keep the state in pf_altq */
172 a->altq_disc = hif;
173
174 return (0);
175 }
176
177 int
hfsc_remove_altq(struct pf_altq * a)178 hfsc_remove_altq(struct pf_altq *a)
179 {
180 struct hfsc_if *hif;
181
182 if ((hif = a->altq_disc) == NULL)
183 return (EINVAL);
184 a->altq_disc = NULL;
185
186 (void)hfsc_clear_interface(hif);
187 (void)hfsc_class_destroy(hif->hif_rootclass);
188
189 ellist_destroy(hif->hif_eligible);
190
191 FREE(hif, M_DEVBUF);
192
193 return (0);
194 }
195
196 int
hfsc_add_queue(struct pf_altq * a)197 hfsc_add_queue(struct pf_altq *a)
198 {
199 struct hfsc_if *hif;
200 struct hfsc_class *cl, *parent;
201 struct hfsc_opts *opts;
202 struct service_curve rtsc, lssc, ulsc;
203
204 if ((hif = a->altq_disc) == NULL)
205 return (EINVAL);
206
207 opts = &a->pq_u.hfsc_opts;
208
209 if (a->parent_qid == HFSC_NULLCLASS_HANDLE &&
210 hif->hif_rootclass == NULL)
211 parent = NULL;
212 else if ((parent = clh_to_clp(hif, a->parent_qid)) == NULL)
213 return (EINVAL);
214
215 if (a->qid == 0)
216 return (EINVAL);
217
218 if (clh_to_clp(hif, a->qid) != NULL)
219 return (EBUSY);
220
221 rtsc.m1 = opts->rtsc_m1;
222 rtsc.d = opts->rtsc_d;
223 rtsc.m2 = opts->rtsc_m2;
224 lssc.m1 = opts->lssc_m1;
225 lssc.d = opts->lssc_d;
226 lssc.m2 = opts->lssc_m2;
227 ulsc.m1 = opts->ulsc_m1;
228 ulsc.d = opts->ulsc_d;
229 ulsc.m2 = opts->ulsc_m2;
230
231 cl = hfsc_class_create(hif, &rtsc, &lssc, &ulsc,
232 parent, a->qlimit, opts->flags, a->qid);
233 if (cl == NULL)
234 return (ENOMEM);
235
236 return (0);
237 }
238
239 int
hfsc_remove_queue(struct pf_altq * a)240 hfsc_remove_queue(struct pf_altq *a)
241 {
242 struct hfsc_if *hif;
243 struct hfsc_class *cl;
244
245 if ((hif = a->altq_disc) == NULL)
246 return (EINVAL);
247
248 if ((cl = clh_to_clp(hif, a->qid)) == NULL)
249 return (EINVAL);
250
251 return (hfsc_class_destroy(cl));
252 }
253
254 int
hfsc_getqstats(struct pf_altq * a,void * ubuf,int * nbytes)255 hfsc_getqstats(struct pf_altq *a, void *ubuf, int *nbytes)
256 {
257 struct hfsc_if *hif;
258 struct hfsc_class *cl;
259 struct hfsc_classstats stats;
260 int error = 0;
261
262 if ((hif = altq_lookup(a->ifname, ALTQT_HFSC)) == NULL)
263 return (EBADF);
264
265 if ((cl = clh_to_clp(hif, a->qid)) == NULL)
266 return (EINVAL);
267
268 if (*nbytes < sizeof(stats))
269 return (EINVAL);
270
271 get_class_stats(&stats, cl);
272
273 if ((error = copyout((caddr_t)&stats, ubuf, sizeof(stats))) != 0)
274 return (error);
275 *nbytes = sizeof(stats);
276 return (0);
277 }
278
279 /*
280 * bring the interface back to the initial state by discarding
281 * all the filters and classes except the root class.
282 */
283 static int
hfsc_clear_interface(struct hfsc_if * hif)284 hfsc_clear_interface(struct hfsc_if *hif)
285 {
286 struct hfsc_class *cl;
287
288 /* clear out the classes */
289 while (hif->hif_rootclass != NULL &&
290 (cl = hif->hif_rootclass->cl_children) != NULL) {
291 /*
292 * remove the first leaf class found in the hierarchy
293 * then start over
294 */
295 for (; cl != NULL; cl = hfsc_nextclass(cl)) {
296 if (!is_a_parent_class(cl)) {
297 (void)hfsc_class_destroy(cl);
298 break;
299 }
300 }
301 }
302
303 return (0);
304 }
305
306 static int
hfsc_request(struct ifaltq * ifq,int req,void * arg)307 hfsc_request(struct ifaltq *ifq, int req, void *arg)
308 {
309 struct hfsc_if *hif = (struct hfsc_if *)ifq->altq_disc;
310
311 switch (req) {
312 case ALTRQ_PURGE:
313 hfsc_purge(hif);
314 break;
315 }
316 return (0);
317 }
318
319 /* discard all the queued packets on the interface */
320 static void
hfsc_purge(struct hfsc_if * hif)321 hfsc_purge(struct hfsc_if *hif)
322 {
323 struct hfsc_class *cl;
324
325 for (cl = hif->hif_rootclass; cl != NULL; cl = hfsc_nextclass(cl))
326 if (!qempty(cl->cl_q))
327 hfsc_purgeq(cl);
328 if (ALTQ_IS_ENABLED(hif->hif_ifq))
329 hif->hif_ifq->ifq_len = 0;
330 }
331
332 struct hfsc_class *
hfsc_class_create(struct hfsc_if * hif,struct service_curve * rsc,struct service_curve * fsc,struct service_curve * usc,struct hfsc_class * parent,int qlimit,int flags,int qid)333 hfsc_class_create(struct hfsc_if *hif, struct service_curve *rsc,
334 struct service_curve *fsc, struct service_curve *usc,
335 struct hfsc_class *parent, int qlimit, int flags, int qid)
336 {
337 struct hfsc_class *cl, *p;
338 int i, s;
339
340 if (hif->hif_classes >= HFSC_MAX_CLASSES)
341 return (NULL);
342
343 #ifndef ALTQ_RED
344 if (flags & HFCF_RED) {
345 #ifdef ALTQ_DEBUG
346 printf("hfsc_class_create: RED not configured for HFSC!\n");
347 #endif
348 return (NULL);
349 }
350 #endif
351
352 MALLOC(cl, struct hfsc_class *, sizeof(struct hfsc_class),
353 M_DEVBUF, M_WAITOK);
354 if (cl == NULL)
355 return (NULL);
356 bzero(cl, sizeof(struct hfsc_class));
357
358 MALLOC(cl->cl_q, class_queue_t *, sizeof(class_queue_t),
359 M_DEVBUF, M_WAITOK);
360 if (cl->cl_q == NULL)
361 goto err_ret;
362 bzero(cl->cl_q, sizeof(class_queue_t));
363
364 cl->cl_actc = actlist_alloc();
365 if (cl->cl_actc == NULL)
366 goto err_ret;
367
368 if (qlimit == 0)
369 qlimit = 50; /* use default */
370 qlimit(cl->cl_q) = qlimit;
371 qtype(cl->cl_q) = Q_DROPTAIL;
372 qlen(cl->cl_q) = 0;
373 cl->cl_flags = flags;
374 #ifdef ALTQ_RED
375 if (flags & (HFCF_RED|HFCF_RIO)) {
376 int red_flags, red_pkttime;
377 u_int m2;
378
379 m2 = 0;
380 if (rsc != NULL && rsc->m2 > m2)
381 m2 = rsc->m2;
382 if (fsc != NULL && fsc->m2 > m2)
383 m2 = fsc->m2;
384 if (usc != NULL && usc->m2 > m2)
385 m2 = usc->m2;
386
387 red_flags = 0;
388 if (flags & HFCF_ECN)
389 red_flags |= REDF_ECN;
390 #ifdef ALTQ_RIO
391 if (flags & HFCF_CLEARDSCP)
392 red_flags |= RIOF_CLEARDSCP;
393 #endif
394 if (m2 < 8)
395 red_pkttime = 1000 * 1000 * 1000; /* 1 sec */
396 else
397 red_pkttime = (int64_t)hif->hif_ifq->altq_ifp->if_mtu
398 * 1000 * 1000 * 1000 / (m2 / 8);
399 if (flags & HFCF_RED) {
400 cl->cl_red = red_alloc(0, 0,
401 qlimit(cl->cl_q) * 10/100,
402 qlimit(cl->cl_q) * 30/100,
403 red_flags, red_pkttime);
404 if (cl->cl_red != NULL)
405 qtype(cl->cl_q) = Q_RED;
406 }
407 #ifdef ALTQ_RIO
408 else {
409 cl->cl_red = (red_t *)rio_alloc(0, NULL,
410 red_flags, red_pkttime);
411 if (cl->cl_red != NULL)
412 qtype(cl->cl_q) = Q_RIO;
413 }
414 #endif
415 }
416 #endif /* ALTQ_RED */
417
418 if (rsc != NULL && (rsc->m1 != 0 || rsc->m2 != 0)) {
419 MALLOC(cl->cl_rsc, struct internal_sc *,
420 sizeof(struct internal_sc), M_DEVBUF, M_WAITOK);
421 if (cl->cl_rsc == NULL)
422 goto err_ret;
423 sc2isc(rsc, cl->cl_rsc);
424 rtsc_init(&cl->cl_deadline, cl->cl_rsc, 0, 0);
425 rtsc_init(&cl->cl_eligible, cl->cl_rsc, 0, 0);
426 }
427 if (fsc != NULL && (fsc->m1 != 0 || fsc->m2 != 0)) {
428 MALLOC(cl->cl_fsc, struct internal_sc *,
429 sizeof(struct internal_sc), M_DEVBUF, M_WAITOK);
430 if (cl->cl_fsc == NULL)
431 goto err_ret;
432 sc2isc(fsc, cl->cl_fsc);
433 rtsc_init(&cl->cl_virtual, cl->cl_fsc, 0, 0);
434 }
435 if (usc != NULL && (usc->m1 != 0 || usc->m2 != 0)) {
436 MALLOC(cl->cl_usc, struct internal_sc *,
437 sizeof(struct internal_sc), M_DEVBUF, M_WAITOK);
438 if (cl->cl_usc == NULL)
439 goto err_ret;
440 sc2isc(usc, cl->cl_usc);
441 rtsc_init(&cl->cl_ulimit, cl->cl_usc, 0, 0);
442 }
443
444 cl->cl_id = hif->hif_classid++;
445 cl->cl_handle = qid;
446 cl->cl_hif = hif;
447 cl->cl_parent = parent;
448
449 s = splimp();
450 hif->hif_classes++;
451
452 /*
453 * find a free slot in the class table. if the slot matching
454 * the lower bits of qid is free, use this slot. otherwise,
455 * use the first free slot.
456 */
457 i = qid % HFSC_MAX_CLASSES;
458 if (hif->hif_class_tbl[i] == NULL)
459 hif->hif_class_tbl[i] = cl;
460 else {
461 for (i = 0; i < HFSC_MAX_CLASSES; i++)
462 if (hif->hif_class_tbl[i] == NULL) {
463 hif->hif_class_tbl[i] = cl;
464 break;
465 }
466 if (i == HFSC_MAX_CLASSES) {
467 splx(s);
468 goto err_ret;
469 }
470 }
471
472 if (flags & HFCF_DEFAULTCLASS)
473 hif->hif_defaultclass = cl;
474
475 if (parent == NULL) {
476 /* this is root class */
477 hif->hif_rootclass = cl;
478 } else {
479 /* add this class to the children list of the parent */
480 if ((p = parent->cl_children) == NULL)
481 parent->cl_children = cl;
482 else {
483 while (p->cl_siblings != NULL)
484 p = p->cl_siblings;
485 p->cl_siblings = cl;
486 }
487 }
488 splx(s);
489
490 return (cl);
491
492 err_ret:
493 if (cl->cl_actc != NULL)
494 actlist_destroy(cl->cl_actc);
495 if (cl->cl_red != NULL) {
496 #ifdef ALTQ_RIO
497 if (q_is_rio(cl->cl_q))
498 rio_destroy((rio_t *)cl->cl_red);
499 #endif
500 #ifdef ALTQ_RED
501 if (q_is_red(cl->cl_q))
502 red_destroy(cl->cl_red);
503 #endif
504 }
505 if (cl->cl_fsc != NULL)
506 FREE(cl->cl_fsc, M_DEVBUF);
507 if (cl->cl_rsc != NULL)
508 FREE(cl->cl_rsc, M_DEVBUF);
509 if (cl->cl_usc != NULL)
510 FREE(cl->cl_usc, M_DEVBUF);
511 if (cl->cl_q != NULL)
512 FREE(cl->cl_q, M_DEVBUF);
513 FREE(cl, M_DEVBUF);
514 return (NULL);
515 }
516
517 static int
hfsc_class_destroy(struct hfsc_class * cl)518 hfsc_class_destroy(struct hfsc_class *cl)
519 {
520 int i, s;
521
522 if (cl == NULL)
523 return (0);
524
525 if (is_a_parent_class(cl))
526 return (EBUSY);
527
528 s = splimp();
529
530 if (!qempty(cl->cl_q))
531 hfsc_purgeq(cl);
532
533 if (cl->cl_parent == NULL) {
534 /* this is root class */
535 } else {
536 struct hfsc_class *p = cl->cl_parent->cl_children;
537
538 if (p == cl)
539 cl->cl_parent->cl_children = cl->cl_siblings;
540 else do {
541 if (p->cl_siblings == cl) {
542 p->cl_siblings = cl->cl_siblings;
543 break;
544 }
545 } while ((p = p->cl_siblings) != NULL);
546 ASSERT(p != NULL);
547 }
548
549 for (i = 0; i < HFSC_MAX_CLASSES; i++)
550 if (cl->cl_hif->hif_class_tbl[i] == cl) {
551 cl->cl_hif->hif_class_tbl[i] = NULL;
552 break;
553 }
554
555 cl->cl_hif->hif_classes--;
556 splx(s);
557
558 actlist_destroy(cl->cl_actc);
559
560 if (cl->cl_red != NULL) {
561 #ifdef ALTQ_RIO
562 if (q_is_rio(cl->cl_q))
563 rio_destroy((rio_t *)cl->cl_red);
564 #endif
565 #ifdef ALTQ_RED
566 if (q_is_red(cl->cl_q))
567 red_destroy(cl->cl_red);
568 #endif
569 }
570
571 if (cl == cl->cl_hif->hif_rootclass)
572 cl->cl_hif->hif_rootclass = NULL;
573 if (cl == cl->cl_hif->hif_defaultclass)
574 cl->cl_hif->hif_defaultclass = NULL;
575
576 if (cl->cl_usc != NULL)
577 FREE(cl->cl_usc, M_DEVBUF);
578 if (cl->cl_fsc != NULL)
579 FREE(cl->cl_fsc, M_DEVBUF);
580 if (cl->cl_rsc != NULL)
581 FREE(cl->cl_rsc, M_DEVBUF);
582 FREE(cl->cl_q, M_DEVBUF);
583 FREE(cl, M_DEVBUF);
584
585 return (0);
586 }
587
588 /*
589 * hfsc_nextclass returns the next class in the tree.
590 * usage:
591 * for (cl = hif->hif_rootclass; cl != NULL; cl = hfsc_nextclass(cl))
592 * do_something;
593 */
594 static struct hfsc_class *
hfsc_nextclass(struct hfsc_class * cl)595 hfsc_nextclass(struct hfsc_class *cl)
596 {
597 if (cl->cl_children != NULL)
598 cl = cl->cl_children;
599 else if (cl->cl_siblings != NULL)
600 cl = cl->cl_siblings;
601 else {
602 while ((cl = cl->cl_parent) != NULL)
603 if (cl->cl_siblings) {
604 cl = cl->cl_siblings;
605 break;
606 }
607 }
608
609 return (cl);
610 }
611
612 /*
613 * hfsc_enqueue is an enqueue function to be registered to
614 * (*altq_enqueue) in struct ifaltq.
615 */
616 static int
hfsc_enqueue(struct ifaltq * ifq,struct mbuf * m,struct altq_pktattr * pktattr)617 hfsc_enqueue(struct ifaltq *ifq, struct mbuf *m, struct altq_pktattr *pktattr)
618 {
619 struct hfsc_if *hif = (struct hfsc_if *)ifq->altq_disc;
620 struct hfsc_class *cl;
621 struct m_tag *t;
622 int len;
623
624 /* grab class set by classifier */
625 if ((m->m_flags & M_PKTHDR) == 0) {
626 /* should not happen */
627 printf("altq: packet for %s does not have pkthdr\n",
628 ifq->altq_ifp->if_xname);
629 m_freem(m);
630 return (ENOBUFS);
631 }
632 t = m_tag_find(m, PACKET_TAG_PF_QID, NULL);
633 if (t == NULL ||
634 (cl = clh_to_clp(hif, ((struct altq_tag *)(t+1))->qid)) == NULL ||
635 is_a_parent_class(cl)) {
636 cl = hif->hif_defaultclass;
637 if (cl == NULL) {
638 m_freem(m);
639 return (ENOBUFS);
640 }
641 cl->cl_pktattr = NULL;
642 }
643
644 len = m_pktlen(m);
645 if (hfsc_addq(cl, m) != 0) {
646 /* drop occurred. mbuf was freed in hfsc_addq. */
647 PKTCNTR_ADD(&cl->cl_stats.drop_cnt, len);
648 return (ENOBUFS);
649 }
650 IFQ_INC_LEN(ifq);
651 cl->cl_hif->hif_packets++;
652
653 /* successfully queued. */
654 if (qlen(cl->cl_q) == 1)
655 set_active(cl, m_pktlen(m));
656
657 return (0);
658 }
659
660 /*
661 * hfsc_dequeue is a dequeue function to be registered to
662 * (*altq_dequeue) in struct ifaltq.
663 *
664 * note: ALTDQ_POLL returns the next packet without removing the packet
665 * from the queue. ALTDQ_REMOVE is a normal dequeue operation.
666 * ALTDQ_REMOVE must return the same packet if called immediately
667 * after ALTDQ_POLL.
668 */
669 static struct mbuf *
hfsc_dequeue(struct ifaltq * ifq,int op)670 hfsc_dequeue(struct ifaltq *ifq, int op)
671 {
672 struct hfsc_if *hif = (struct hfsc_if *)ifq->altq_disc;
673 struct hfsc_class *cl;
674 struct mbuf *m;
675 int len, next_len;
676 int realtime = 0;
677 u_int64_t cur_time;
678
679 if (hif->hif_packets == 0)
680 /* no packet in the tree */
681 return (NULL);
682
683 cur_time = read_machclk();
684
685 if (op == ALTDQ_REMOVE && hif->hif_pollcache != NULL) {
686
687 cl = hif->hif_pollcache;
688 hif->hif_pollcache = NULL;
689 /* check if the class was scheduled by real-time criteria */
690 if (cl->cl_rsc != NULL)
691 realtime = (cl->cl_e <= cur_time);
692 } else {
693 /*
694 * if there are eligible classes, use real-time criteria.
695 * find the class with the minimum deadline among
696 * the eligible classes.
697 */
698 if ((cl = ellist_get_mindl(hif->hif_eligible, cur_time))
699 != NULL) {
700 realtime = 1;
701 } else {
702 #ifdef ALTQ_DEBUG
703 int fits = 0;
704 #endif
705 /*
706 * use link-sharing criteria
707 * get the class with the minimum vt in the hierarchy
708 */
709 cl = hif->hif_rootclass;
710 while (is_a_parent_class(cl)) {
711
712 cl = actlist_firstfit(cl, cur_time);
713 if (cl == NULL) {
714 #ifdef ALTQ_DEBUG
715 if (fits > 0)
716 printf("%d fit but none found\n",fits);
717 #endif
718 return (NULL);
719 }
720 /*
721 * update parent's cl_cvtmin.
722 * don't update if the new vt is smaller.
723 */
724 if (cl->cl_parent->cl_cvtmin < cl->cl_vt)
725 cl->cl_parent->cl_cvtmin = cl->cl_vt;
726 #ifdef ALTQ_DEBUG
727 fits++;
728 #endif
729 }
730 }
731
732 if (op == ALTDQ_POLL) {
733 hif->hif_pollcache = cl;
734 m = hfsc_pollq(cl);
735 return (m);
736 }
737 }
738
739 m = hfsc_getq(cl);
740 if (m == NULL)
741 panic("hfsc_dequeue:");
742 len = m_pktlen(m);
743 cl->cl_hif->hif_packets--;
744 IFQ_DEC_LEN(ifq);
745 PKTCNTR_ADD(&cl->cl_stats.xmit_cnt, len);
746
747 update_vf(cl, len, cur_time);
748 if (realtime)
749 cl->cl_cumul += len;
750
751 if (!qempty(cl->cl_q)) {
752 if (cl->cl_rsc != NULL) {
753 /* update ed */
754 next_len = m_pktlen(qhead(cl->cl_q));
755
756 if (realtime)
757 update_ed(cl, next_len);
758 else
759 update_d(cl, next_len);
760 }
761 } else {
762 /* the class becomes passive */
763 set_passive(cl);
764 }
765
766 return (m);
767 }
768
769 static int
hfsc_addq(struct hfsc_class * cl,struct mbuf * m)770 hfsc_addq(struct hfsc_class *cl, struct mbuf *m)
771 {
772
773 #ifdef ALTQ_RIO
774 if (q_is_rio(cl->cl_q))
775 return rio_addq((rio_t *)cl->cl_red, cl->cl_q,
776 m, cl->cl_pktattr);
777 #endif
778 #ifdef ALTQ_RED
779 if (q_is_red(cl->cl_q))
780 return red_addq(cl->cl_red, cl->cl_q, m, cl->cl_pktattr);
781 #endif
782 if (qlen(cl->cl_q) >= qlimit(cl->cl_q)) {
783 m_freem(m);
784 return (-1);
785 }
786
787 if (cl->cl_flags & HFCF_CLEARDSCP)
788 write_dsfield(m, cl->cl_pktattr, 0);
789
790 _addq(cl->cl_q, m);
791
792 return (0);
793 }
794
795 static struct mbuf *
hfsc_getq(struct hfsc_class * cl)796 hfsc_getq(struct hfsc_class *cl)
797 {
798 #ifdef ALTQ_RIO
799 if (q_is_rio(cl->cl_q))
800 return rio_getq((rio_t *)cl->cl_red, cl->cl_q);
801 #endif
802 #ifdef ALTQ_RED
803 if (q_is_red(cl->cl_q))
804 return red_getq(cl->cl_red, cl->cl_q);
805 #endif
806 return _getq(cl->cl_q);
807 }
808
809 static struct mbuf *
hfsc_pollq(struct hfsc_class * cl)810 hfsc_pollq(struct hfsc_class *cl)
811 {
812 return qhead(cl->cl_q);
813 }
814
815 static void
hfsc_purgeq(struct hfsc_class * cl)816 hfsc_purgeq(struct hfsc_class *cl)
817 {
818 struct mbuf *m;
819
820 if (qempty(cl->cl_q))
821 return;
822
823 while ((m = _getq(cl->cl_q)) != NULL) {
824 PKTCNTR_ADD(&cl->cl_stats.drop_cnt, m_pktlen(m));
825 m_freem(m);
826 cl->cl_hif->hif_packets--;
827 IFQ_DEC_LEN(cl->cl_hif->hif_ifq);
828 }
829 ASSERT(qlen(cl->cl_q) == 0);
830
831 update_vf(cl, 0, 0); /* remove cl from the actlist */
832 set_passive(cl);
833 }
834
835 static void
set_active(struct hfsc_class * cl,int len)836 set_active(struct hfsc_class *cl, int len)
837 {
838 if (cl->cl_rsc != NULL)
839 init_ed(cl, len);
840 if (cl->cl_fsc != NULL)
841 init_vf(cl, len);
842
843 cl->cl_stats.period++;
844 }
845
846 static void
set_passive(struct hfsc_class * cl)847 set_passive(struct hfsc_class *cl)
848 {
849 if (cl->cl_rsc != NULL)
850 ellist_remove(cl);
851
852 /*
853 * actlist is now handled in update_vf() so that update_vf(cl, 0, 0)
854 * needs to be called explicitly to remove a class from actlist
855 */
856 }
857
858 static void
init_ed(struct hfsc_class * cl,int next_len)859 init_ed(struct hfsc_class *cl, int next_len)
860 {
861 u_int64_t cur_time;
862
863 cur_time = read_machclk();
864
865 /* update the deadline curve */
866 rtsc_min(&cl->cl_deadline, cl->cl_rsc, cur_time, cl->cl_cumul);
867
868 /*
869 * update the eligible curve.
870 * for concave, it is equal to the deadline curve.
871 * for convex, it is a linear curve with slope m2.
872 */
873 cl->cl_eligible = cl->cl_deadline;
874 if (cl->cl_rsc->sm1 <= cl->cl_rsc->sm2) {
875 cl->cl_eligible.dx = 0;
876 cl->cl_eligible.dy = 0;
877 }
878
879 /* compute e and d */
880 cl->cl_e = rtsc_y2x(&cl->cl_eligible, cl->cl_cumul);
881 cl->cl_d = rtsc_y2x(&cl->cl_deadline, cl->cl_cumul + next_len);
882
883 ellist_insert(cl);
884 }
885
886 static void
update_ed(struct hfsc_class * cl,int next_len)887 update_ed(struct hfsc_class *cl, int next_len)
888 {
889 cl->cl_e = rtsc_y2x(&cl->cl_eligible, cl->cl_cumul);
890 cl->cl_d = rtsc_y2x(&cl->cl_deadline, cl->cl_cumul + next_len);
891
892 ellist_update(cl);
893 }
894
895 static void
update_d(struct hfsc_class * cl,int next_len)896 update_d(struct hfsc_class *cl, int next_len)
897 {
898 cl->cl_d = rtsc_y2x(&cl->cl_deadline, cl->cl_cumul + next_len);
899 }
900
901 static void
init_vf(struct hfsc_class * cl,int len)902 init_vf(struct hfsc_class *cl, int len)
903 {
904 struct hfsc_class *max_cl, *p;
905 u_int64_t vt, f, cur_time;
906 int go_active;
907
908 cur_time = 0;
909 go_active = 1;
910 for ( ; cl->cl_parent != NULL; cl = cl->cl_parent) {
911
912 if (go_active && cl->cl_nactive++ == 0)
913 go_active = 1;
914 else
915 go_active = 0;
916
917 if (go_active) {
918 max_cl = actlist_last(cl->cl_parent->cl_actc);
919 if (max_cl != NULL) {
920 /*
921 * set vt to the average of the min and max
922 * classes. if the parent's period didn't
923 * change, don't decrease vt of the class.
924 */
925 vt = max_cl->cl_vt;
926 if (cl->cl_parent->cl_cvtmin != 0)
927 vt = (cl->cl_parent->cl_cvtmin + vt)/2;
928
929 if (cl->cl_parent->cl_vtperiod !=
930 cl->cl_parentperiod || vt > cl->cl_vt)
931 cl->cl_vt = vt;
932 } else {
933 /*
934 * first child for a new parent backlog period.
935 * add parent's cvtmax to vtoff of children
936 * to make a new vt (vtoff + vt) larger than
937 * the vt in the last period for all children.
938 */
939 vt = cl->cl_parent->cl_cvtmax;
940 for (p = cl->cl_parent->cl_children; p != NULL;
941 p = p->cl_siblings)
942 p->cl_vtoff += vt;
943 cl->cl_vt = 0;
944 cl->cl_parent->cl_cvtmax = 0;
945 cl->cl_parent->cl_cvtmin = 0;
946 }
947 cl->cl_initvt = cl->cl_vt;
948
949 /* update the virtual curve */
950 vt = cl->cl_vt + cl->cl_vtoff;
951 rtsc_min(&cl->cl_virtual, cl->cl_fsc, vt, cl->cl_total);
952 if (cl->cl_virtual.x == vt) {
953 cl->cl_virtual.x -= cl->cl_vtoff;
954 cl->cl_vtoff = 0;
955 }
956 cl->cl_vtadj = 0;
957
958 cl->cl_vtperiod++; /* increment vt period */
959 cl->cl_parentperiod = cl->cl_parent->cl_vtperiod;
960 if (cl->cl_parent->cl_nactive == 0)
961 cl->cl_parentperiod++;
962 cl->cl_f = 0;
963
964 actlist_insert(cl);
965
966 if (cl->cl_usc != NULL) {
967 /* class has upper limit curve */
968 if (cur_time == 0)
969 cur_time = read_machclk();
970
971 /* update the ulimit curve */
972 rtsc_min(&cl->cl_ulimit, cl->cl_usc, cur_time,
973 cl->cl_total);
974 /* compute myf */
975 cl->cl_myf = rtsc_y2x(&cl->cl_ulimit,
976 cl->cl_total);
977 cl->cl_myfadj = 0;
978 }
979 }
980
981 if (cl->cl_myf > cl->cl_cfmin)
982 f = cl->cl_myf;
983 else
984 f = cl->cl_cfmin;
985 if (f != cl->cl_f) {
986 cl->cl_f = f;
987 update_cfmin(cl->cl_parent);
988 }
989 }
990 }
991
992 static void
update_vf(struct hfsc_class * cl,int len,u_int64_t cur_time)993 update_vf(struct hfsc_class *cl, int len, u_int64_t cur_time)
994 {
995 u_int64_t f, myf_bound, delta;
996 int go_passive;
997
998 go_passive = qempty(cl->cl_q);
999
1000 for (; cl->cl_parent != NULL; cl = cl->cl_parent) {
1001
1002 cl->cl_total += len;
1003
1004 if (cl->cl_fsc == NULL || cl->cl_nactive == 0)
1005 continue;
1006
1007 if (go_passive && --cl->cl_nactive == 0)
1008 go_passive = 1;
1009 else
1010 go_passive = 0;
1011
1012 if (go_passive) {
1013 /* no more active child, going passive */
1014
1015 /* update cvtmax of the parent class */
1016 if (cl->cl_vt > cl->cl_parent->cl_cvtmax)
1017 cl->cl_parent->cl_cvtmax = cl->cl_vt;
1018
1019 /* remove this class from the vt list */
1020 actlist_remove(cl);
1021
1022 update_cfmin(cl->cl_parent);
1023
1024 continue;
1025 }
1026
1027 /*
1028 * update vt and f
1029 */
1030 cl->cl_vt = rtsc_y2x(&cl->cl_virtual, cl->cl_total)
1031 - cl->cl_vtoff + cl->cl_vtadj;
1032
1033 /*
1034 * if vt of the class is smaller than cvtmin,
1035 * the class was skipped in the past due to non-fit.
1036 * if so, we need to adjust vtadj.
1037 */
1038 if (cl->cl_vt < cl->cl_parent->cl_cvtmin) {
1039 cl->cl_vtadj += cl->cl_parent->cl_cvtmin - cl->cl_vt;
1040 cl->cl_vt = cl->cl_parent->cl_cvtmin;
1041 }
1042
1043 /* update the vt list */
1044 actlist_update(cl);
1045
1046 if (cl->cl_usc != NULL) {
1047 cl->cl_myf = cl->cl_myfadj
1048 + rtsc_y2x(&cl->cl_ulimit, cl->cl_total);
1049
1050 /*
1051 * if myf lags behind by more than one clock tick
1052 * from the current time, adjust myfadj to prevent
1053 * a rate-limited class from going greedy.
1054 * in a steady state under rate-limiting, myf
1055 * fluctuates within one clock tick.
1056 */
1057 myf_bound = cur_time - machclk_per_tick;
1058 if (cl->cl_myf < myf_bound) {
1059 delta = cur_time - cl->cl_myf;
1060 cl->cl_myfadj += delta;
1061 cl->cl_myf += delta;
1062 }
1063 }
1064
1065 /* cl_f is max(cl_myf, cl_cfmin) */
1066 if (cl->cl_myf > cl->cl_cfmin)
1067 f = cl->cl_myf;
1068 else
1069 f = cl->cl_cfmin;
1070 if (f != cl->cl_f) {
1071 cl->cl_f = f;
1072 update_cfmin(cl->cl_parent);
1073 }
1074 }
1075 }
1076
1077 static void
update_cfmin(struct hfsc_class * cl)1078 update_cfmin(struct hfsc_class *cl)
1079 {
1080 struct hfsc_class *p;
1081 u_int64_t cfmin;
1082
1083 if (TAILQ_EMPTY(cl->cl_actc)) {
1084 cl->cl_cfmin = 0;
1085 return;
1086 }
1087 cfmin = HT_INFINITY;
1088 TAILQ_FOREACH(p, cl->cl_actc, cl_actlist) {
1089 if (p->cl_f == 0) {
1090 cl->cl_cfmin = 0;
1091 return;
1092 }
1093 if (p->cl_f < cfmin)
1094 cfmin = p->cl_f;
1095 }
1096 cl->cl_cfmin = cfmin;
1097 }
1098
1099 /*
1100 * TAILQ based ellist and actlist implementation
1101 * (ion wanted to make a calendar queue based implementation)
1102 */
1103 /*
1104 * eligible list holds backlogged classes being sorted by their eligible times.
1105 * there is one eligible list per interface.
1106 */
1107
1108 static ellist_t *
ellist_alloc(void)1109 ellist_alloc(void)
1110 {
1111 ellist_t *head;
1112
1113 MALLOC(head, ellist_t *, sizeof(ellist_t), M_DEVBUF, M_WAITOK);
1114 TAILQ_INIT(head);
1115 return (head);
1116 }
1117
1118 static void
ellist_destroy(ellist_t * head)1119 ellist_destroy(ellist_t *head)
1120 {
1121 FREE(head, M_DEVBUF);
1122 }
1123
1124 static void
ellist_insert(struct hfsc_class * cl)1125 ellist_insert(struct hfsc_class *cl)
1126 {
1127 struct hfsc_if *hif = cl->cl_hif;
1128 struct hfsc_class *p;
1129
1130 /* check the last entry first */
1131 if ((p = TAILQ_LAST(hif->hif_eligible, _eligible)) == NULL ||
1132 p->cl_e <= cl->cl_e) {
1133 TAILQ_INSERT_TAIL(hif->hif_eligible, cl, cl_ellist);
1134 return;
1135 }
1136
1137 TAILQ_FOREACH(p, hif->hif_eligible, cl_ellist) {
1138 if (cl->cl_e < p->cl_e) {
1139 TAILQ_INSERT_BEFORE(p, cl, cl_ellist);
1140 return;
1141 }
1142 }
1143 ASSERT(0); /* should not reach here */
1144 }
1145
1146 static void
ellist_remove(struct hfsc_class * cl)1147 ellist_remove(struct hfsc_class *cl)
1148 {
1149 struct hfsc_if *hif = cl->cl_hif;
1150
1151 TAILQ_REMOVE(hif->hif_eligible, cl, cl_ellist);
1152 }
1153
1154 static void
ellist_update(struct hfsc_class * cl)1155 ellist_update(struct hfsc_class *cl)
1156 {
1157 struct hfsc_if *hif = cl->cl_hif;
1158 struct hfsc_class *p, *last;
1159
1160 /*
1161 * the eligible time of a class increases monotonically.
1162 * if the next entry has a larger eligible time, nothing to do.
1163 */
1164 p = TAILQ_NEXT(cl, cl_ellist);
1165 if (p == NULL || cl->cl_e <= p->cl_e)
1166 return;
1167
1168 /* check the last entry */
1169 last = TAILQ_LAST(hif->hif_eligible, _eligible);
1170 ASSERT(last != NULL);
1171 if (last->cl_e <= cl->cl_e) {
1172 TAILQ_REMOVE(hif->hif_eligible, cl, cl_ellist);
1173 TAILQ_INSERT_TAIL(hif->hif_eligible, cl, cl_ellist);
1174 return;
1175 }
1176
1177 /*
1178 * the new position must be between the next entry
1179 * and the last entry
1180 */
1181 while ((p = TAILQ_NEXT(p, cl_ellist)) != NULL) {
1182 if (cl->cl_e < p->cl_e) {
1183 TAILQ_REMOVE(hif->hif_eligible, cl, cl_ellist);
1184 TAILQ_INSERT_BEFORE(p, cl, cl_ellist);
1185 return;
1186 }
1187 }
1188 ASSERT(0); /* should not reach here */
1189 }
1190
1191 /* find the class with the minimum deadline among the eligible classes */
1192 struct hfsc_class *
ellist_get_mindl(ellist_t * head,u_int64_t cur_time)1193 ellist_get_mindl(ellist_t *head, u_int64_t cur_time)
1194 {
1195 struct hfsc_class *p, *cl = NULL;
1196
1197 TAILQ_FOREACH(p, head, cl_ellist) {
1198 if (p->cl_e > cur_time)
1199 break;
1200 if (cl == NULL || p->cl_d < cl->cl_d)
1201 cl = p;
1202 }
1203 return (cl);
1204 }
1205
1206 /*
1207 * active children list holds backlogged child classes being sorted
1208 * by their virtual time.
1209 * each intermediate class has one active children list.
1210 */
1211 static actlist_t *
actlist_alloc(void)1212 actlist_alloc(void)
1213 {
1214 actlist_t *head;
1215
1216 MALLOC(head, actlist_t *, sizeof(actlist_t), M_DEVBUF, M_WAITOK);
1217 TAILQ_INIT(head);
1218 return (head);
1219 }
1220
1221 static void
actlist_destroy(actlist_t * head)1222 actlist_destroy(actlist_t *head)
1223 {
1224 FREE(head, M_DEVBUF);
1225 }
1226 static void
actlist_insert(struct hfsc_class * cl)1227 actlist_insert(struct hfsc_class *cl)
1228 {
1229 struct hfsc_class *p;
1230
1231 /* check the last entry first */
1232 if ((p = TAILQ_LAST(cl->cl_parent->cl_actc, _active)) == NULL
1233 || p->cl_vt <= cl->cl_vt) {
1234 TAILQ_INSERT_TAIL(cl->cl_parent->cl_actc, cl, cl_actlist);
1235 return;
1236 }
1237
1238 TAILQ_FOREACH(p, cl->cl_parent->cl_actc, cl_actlist) {
1239 if (cl->cl_vt < p->cl_vt) {
1240 TAILQ_INSERT_BEFORE(p, cl, cl_actlist);
1241 return;
1242 }
1243 }
1244 ASSERT(0); /* should not reach here */
1245 }
1246
1247 static void
actlist_remove(struct hfsc_class * cl)1248 actlist_remove(struct hfsc_class *cl)
1249 {
1250 TAILQ_REMOVE(cl->cl_parent->cl_actc, cl, cl_actlist);
1251 }
1252
1253 static void
actlist_update(struct hfsc_class * cl)1254 actlist_update(struct hfsc_class *cl)
1255 {
1256 struct hfsc_class *p, *last;
1257
1258 /*
1259 * the virtual time of a class increases monotonically during its
1260 * backlogged period.
1261 * if the next entry has a larger virtual time, nothing to do.
1262 */
1263 p = TAILQ_NEXT(cl, cl_actlist);
1264 if (p == NULL || cl->cl_vt < p->cl_vt)
1265 return;
1266
1267 /* check the last entry */
1268 last = TAILQ_LAST(cl->cl_parent->cl_actc, _active);
1269 ASSERT(last != NULL);
1270 if (last->cl_vt <= cl->cl_vt) {
1271 TAILQ_REMOVE(cl->cl_parent->cl_actc, cl, cl_actlist);
1272 TAILQ_INSERT_TAIL(cl->cl_parent->cl_actc, cl, cl_actlist);
1273 return;
1274 }
1275
1276 /*
1277 * the new position must be between the next entry
1278 * and the last entry
1279 */
1280 while ((p = TAILQ_NEXT(p, cl_actlist)) != NULL) {
1281 if (cl->cl_vt < p->cl_vt) {
1282 TAILQ_REMOVE(cl->cl_parent->cl_actc, cl, cl_actlist);
1283 TAILQ_INSERT_BEFORE(p, cl, cl_actlist);
1284 return;
1285 }
1286 }
1287 ASSERT(0); /* should not reach here */
1288 }
1289
1290 static struct hfsc_class *
actlist_firstfit(struct hfsc_class * cl,u_int64_t cur_time)1291 actlist_firstfit(struct hfsc_class *cl, u_int64_t cur_time)
1292 {
1293 struct hfsc_class *p;
1294
1295 TAILQ_FOREACH(p, cl->cl_actc, cl_actlist) {
1296 if (p->cl_f <= cur_time)
1297 return (p);
1298 }
1299 return (NULL);
1300 }
1301
1302 /*
1303 * service curve support functions
1304 *
1305 * external service curve parameters
1306 * m: bits/sec
1307 * d: msec
1308 * internal service curve parameters
1309 * sm: (bytes/tsc_interval) << SM_SHIFT
1310 * ism: (tsc_count/byte) << ISM_SHIFT
1311 * dx: tsc_count
1312 *
1313 * SM_SHIFT and ISM_SHIFT are scaled in order to keep effective digits.
1314 * we should be able to handle 100K-1Gbps linkspeed with 200Hz-1GHz CPU
1315 * speed. SM_SHIFT and ISM_SHIFT are selected to have at least 3 effective
1316 * digits in decimal using the following table.
1317 *
1318 * bits/sec 100Kbps 1Mbps 10Mbps 100Mbps 1Gbps
1319 * ----------+-------------------------------------------------------
1320 * bytes/nsec 12.5e-6 125e-6 1250e-6 12500e-6 125000e-6
1321 * sm(500MHz) 25.0e-6 250e-6 2500e-6 25000e-6 250000e-6
1322 * sm(200MHz) 62.5e-6 625e-6 6250e-6 62500e-6 625000e-6
1323 *
1324 * nsec/byte 80000 8000 800 80 8
1325 * ism(500MHz) 40000 4000 400 40 4
1326 * ism(200MHz) 16000 1600 160 16 1.6
1327 */
1328 #define SM_SHIFT 24
1329 #define ISM_SHIFT 10
1330
1331 #define SM_MASK ((1LL << SM_SHIFT) - 1)
1332 #define ISM_MASK ((1LL << ISM_SHIFT) - 1)
1333
1334 static __inline u_int64_t
seg_x2y(u_int64_t x,u_int64_t sm)1335 seg_x2y(u_int64_t x, u_int64_t sm)
1336 {
1337 u_int64_t y;
1338
1339 /*
1340 * compute
1341 * y = x * sm >> SM_SHIFT
1342 * but divide it for the upper and lower bits to avoid overflow
1343 */
1344 y = (x >> SM_SHIFT) * sm + (((x & SM_MASK) * sm) >> SM_SHIFT);
1345 return (y);
1346 }
1347
1348 static __inline u_int64_t
seg_y2x(u_int64_t y,u_int64_t ism)1349 seg_y2x(u_int64_t y, u_int64_t ism)
1350 {
1351 u_int64_t x;
1352
1353 if (y == 0)
1354 x = 0;
1355 else if (ism == HT_INFINITY)
1356 x = HT_INFINITY;
1357 else {
1358 x = (y >> ISM_SHIFT) * ism
1359 + (((y & ISM_MASK) * ism) >> ISM_SHIFT);
1360 }
1361 return (x);
1362 }
1363
1364 static __inline u_int64_t
m2sm(u_int m)1365 m2sm(u_int m)
1366 {
1367 u_int64_t sm;
1368
1369 sm = ((u_int64_t)m << SM_SHIFT) / 8 / machclk_freq;
1370 return (sm);
1371 }
1372
1373 static __inline u_int64_t
m2ism(u_int m)1374 m2ism(u_int m)
1375 {
1376 u_int64_t ism;
1377
1378 if (m == 0)
1379 ism = HT_INFINITY;
1380 else
1381 ism = ((u_int64_t)machclk_freq << ISM_SHIFT) * 8 / m;
1382 return (ism);
1383 }
1384
1385 static __inline u_int64_t
d2dx(u_int d)1386 d2dx(u_int d)
1387 {
1388 u_int64_t dx;
1389
1390 dx = ((u_int64_t)d * machclk_freq) / 1000;
1391 return (dx);
1392 }
1393
1394 static u_int
sm2m(u_int64_t sm)1395 sm2m(u_int64_t sm)
1396 {
1397 u_int64_t m;
1398
1399 m = (sm * 8 * machclk_freq) >> SM_SHIFT;
1400 return ((u_int)m);
1401 }
1402
1403 static u_int
dx2d(u_int64_t dx)1404 dx2d(u_int64_t dx)
1405 {
1406 u_int64_t d;
1407
1408 d = dx * 1000 / machclk_freq;
1409 return ((u_int)d);
1410 }
1411
1412 static void
sc2isc(struct service_curve * sc,struct internal_sc * isc)1413 sc2isc(struct service_curve *sc, struct internal_sc *isc)
1414 {
1415 isc->sm1 = m2sm(sc->m1);
1416 isc->ism1 = m2ism(sc->m1);
1417 isc->dx = d2dx(sc->d);
1418 isc->dy = seg_x2y(isc->dx, isc->sm1);
1419 isc->sm2 = m2sm(sc->m2);
1420 isc->ism2 = m2ism(sc->m2);
1421 }
1422
1423 /*
1424 * initialize the runtime service curve with the given internal
1425 * service curve starting at (x, y).
1426 */
1427 static void
rtsc_init(struct runtime_sc * rtsc,struct internal_sc * isc,u_int64_t x,u_int64_t y)1428 rtsc_init(struct runtime_sc *rtsc, struct internal_sc * isc, u_int64_t x,
1429 u_int64_t y)
1430 {
1431 rtsc->x = x;
1432 rtsc->y = y;
1433 rtsc->sm1 = isc->sm1;
1434 rtsc->ism1 = isc->ism1;
1435 rtsc->dx = isc->dx;
1436 rtsc->dy = isc->dy;
1437 rtsc->sm2 = isc->sm2;
1438 rtsc->ism2 = isc->ism2;
1439 }
1440
1441 /*
1442 * calculate the y-projection of the runtime service curve by the
1443 * given x-projection value
1444 */
1445 static u_int64_t
rtsc_y2x(struct runtime_sc * rtsc,u_int64_t y)1446 rtsc_y2x(struct runtime_sc *rtsc, u_int64_t y)
1447 {
1448 u_int64_t x;
1449
1450 if (y < rtsc->y)
1451 x = rtsc->x;
1452 else if (y <= rtsc->y + rtsc->dy) {
1453 /* x belongs to the 1st segment */
1454 if (rtsc->dy == 0)
1455 x = rtsc->x + rtsc->dx;
1456 else
1457 x = rtsc->x + seg_y2x(y - rtsc->y, rtsc->ism1);
1458 } else {
1459 /* x belongs to the 2nd segment */
1460 x = rtsc->x + rtsc->dx
1461 + seg_y2x(y - rtsc->y - rtsc->dy, rtsc->ism2);
1462 }
1463 return (x);
1464 }
1465
1466 static u_int64_t
rtsc_x2y(struct runtime_sc * rtsc,u_int64_t x)1467 rtsc_x2y(struct runtime_sc *rtsc, u_int64_t x)
1468 {
1469 u_int64_t y;
1470
1471 if (x <= rtsc->x)
1472 y = rtsc->y;
1473 else if (x <= rtsc->x + rtsc->dx)
1474 /* y belongs to the 1st segment */
1475 y = rtsc->y + seg_x2y(x - rtsc->x, rtsc->sm1);
1476 else
1477 /* y belongs to the 2nd segment */
1478 y = rtsc->y + rtsc->dy
1479 + seg_x2y(x - rtsc->x - rtsc->dx, rtsc->sm2);
1480 return (y);
1481 }
1482
1483 /*
1484 * update the runtime service curve by taking the minimum of the current
1485 * runtime service curve and the service curve starting at (x, y).
1486 */
1487 static void
rtsc_min(struct runtime_sc * rtsc,struct internal_sc * isc,u_int64_t x,u_int64_t y)1488 rtsc_min(struct runtime_sc *rtsc, struct internal_sc *isc, u_int64_t x,
1489 u_int64_t y)
1490 {
1491 u_int64_t y1, y2, dx, dy;
1492
1493 if (isc->sm1 <= isc->sm2) {
1494 /* service curve is convex */
1495 y1 = rtsc_x2y(rtsc, x);
1496 if (y1 < y)
1497 /* the current rtsc is smaller */
1498 return;
1499 rtsc->x = x;
1500 rtsc->y = y;
1501 return;
1502 }
1503
1504 /*
1505 * service curve is concave
1506 * compute the two y values of the current rtsc
1507 * y1: at x
1508 * y2: at (x + dx)
1509 */
1510 y1 = rtsc_x2y(rtsc, x);
1511 if (y1 <= y) {
1512 /* rtsc is below isc, no change to rtsc */
1513 return;
1514 }
1515
1516 y2 = rtsc_x2y(rtsc, x + isc->dx);
1517 if (y2 >= y + isc->dy) {
1518 /* rtsc is above isc, replace rtsc by isc */
1519 rtsc->x = x;
1520 rtsc->y = y;
1521 rtsc->dx = isc->dx;
1522 rtsc->dy = isc->dy;
1523 return;
1524 }
1525
1526 /*
1527 * the two curves intersect
1528 * compute the offsets (dx, dy) using the reverse
1529 * function of seg_x2y()
1530 * seg_x2y(dx, sm1) == seg_x2y(dx, sm2) + (y1 - y)
1531 */
1532 dx = ((y1 - y) << SM_SHIFT) / (isc->sm1 - isc->sm2);
1533 /*
1534 * check if (x, y1) belongs to the 1st segment of rtsc.
1535 * if so, add the offset.
1536 */
1537 if (rtsc->x + rtsc->dx > x)
1538 dx += rtsc->x + rtsc->dx - x;
1539 dy = seg_x2y(dx, isc->sm1);
1540
1541 rtsc->x = x;
1542 rtsc->y = y;
1543 rtsc->dx = dx;
1544 rtsc->dy = dy;
1545 return;
1546 }
1547
1548 static void
get_class_stats(struct hfsc_classstats * sp,struct hfsc_class * cl)1549 get_class_stats(struct hfsc_classstats *sp, struct hfsc_class *cl)
1550 {
1551 sp->class_id = cl->cl_id;
1552 sp->class_handle = cl->cl_handle;
1553
1554 if (cl->cl_rsc != NULL) {
1555 sp->rsc.m1 = sm2m(cl->cl_rsc->sm1);
1556 sp->rsc.d = dx2d(cl->cl_rsc->dx);
1557 sp->rsc.m2 = sm2m(cl->cl_rsc->sm2);
1558 } else {
1559 sp->rsc.m1 = 0;
1560 sp->rsc.d = 0;
1561 sp->rsc.m2 = 0;
1562 }
1563 if (cl->cl_fsc != NULL) {
1564 sp->fsc.m1 = sm2m(cl->cl_fsc->sm1);
1565 sp->fsc.d = dx2d(cl->cl_fsc->dx);
1566 sp->fsc.m2 = sm2m(cl->cl_fsc->sm2);
1567 } else {
1568 sp->fsc.m1 = 0;
1569 sp->fsc.d = 0;
1570 sp->fsc.m2 = 0;
1571 }
1572 if (cl->cl_usc != NULL) {
1573 sp->usc.m1 = sm2m(cl->cl_usc->sm1);
1574 sp->usc.d = dx2d(cl->cl_usc->dx);
1575 sp->usc.m2 = sm2m(cl->cl_usc->sm2);
1576 } else {
1577 sp->usc.m1 = 0;
1578 sp->usc.d = 0;
1579 sp->usc.m2 = 0;
1580 }
1581
1582 sp->total = cl->cl_total;
1583 sp->cumul = cl->cl_cumul;
1584
1585 sp->d = cl->cl_d;
1586 sp->e = cl->cl_e;
1587 sp->vt = cl->cl_vt;
1588 sp->f = cl->cl_f;
1589
1590 sp->initvt = cl->cl_initvt;
1591 sp->vtperiod = cl->cl_vtperiod;
1592 sp->parentperiod = cl->cl_parentperiod;
1593 sp->nactive = cl->cl_nactive;
1594 sp->vtoff = cl->cl_vtoff;
1595 sp->cvtmax = cl->cl_cvtmax;
1596 sp->myf = cl->cl_myf;
1597 sp->cfmin = cl->cl_cfmin;
1598 sp->cvtmin = cl->cl_cvtmin;
1599 sp->myfadj = cl->cl_myfadj;
1600 sp->vtadj = cl->cl_vtadj;
1601
1602 sp->cur_time = read_machclk();
1603 sp->machclk_freq = machclk_freq;
1604
1605 sp->qlength = qlen(cl->cl_q);
1606 sp->qlimit = qlimit(cl->cl_q);
1607 sp->xmit_cnt = cl->cl_stats.xmit_cnt;
1608 sp->drop_cnt = cl->cl_stats.drop_cnt;
1609 sp->period = cl->cl_stats.period;
1610
1611 sp->qtype = qtype(cl->cl_q);
1612 #ifdef ALTQ_RED
1613 if (q_is_red(cl->cl_q))
1614 red_getstats(cl->cl_red, &sp->red[0]);
1615 #endif
1616 #ifdef ALTQ_RIO
1617 if (q_is_rio(cl->cl_q))
1618 rio_getstats((rio_t *)cl->cl_red, &sp->red[0]);
1619 #endif
1620 }
1621
1622 /* convert a class handle to the corresponding class pointer */
1623 static struct hfsc_class *
clh_to_clp(struct hfsc_if * hif,u_int32_t chandle)1624 clh_to_clp(struct hfsc_if *hif, u_int32_t chandle)
1625 {
1626 int i;
1627 struct hfsc_class *cl;
1628
1629 if (chandle == 0)
1630 return (NULL);
1631 /*
1632 * first, try the slot corresponding to the lower bits of the handle.
1633 * if it does not match, do the linear table search.
1634 */
1635 i = chandle % HFSC_MAX_CLASSES;
1636 if ((cl = hif->hif_class_tbl[i]) != NULL && cl->cl_handle == chandle)
1637 return (cl);
1638 for (i = 0; i < HFSC_MAX_CLASSES; i++)
1639 if ((cl = hif->hif_class_tbl[i]) != NULL &&
1640 cl->cl_handle == chandle)
1641 return (cl);
1642 return (NULL);
1643 }
1644