1/*	$NetBSD: qop_hfsc.c,v 1.8 2006/10/12 19:59:13 peter Exp $	*/
2/*	$KAME: qop_hfsc.c,v 1.12 2005/01/05 04:53:47 itojun Exp $	*/
3/*
4 * Copyright (C) 1999-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#include <sys/param.h>
30#include <sys/socket.h>
31#include <sys/sockio.h>
32#include <sys/ioctl.h>
33#include <sys/fcntl.h>
34#include <net/if.h>
35#include <netinet/in.h>
36#include <arpa/inet.h>
37
38#include <stdio.h>
39#include <stdlib.h>
40#include <unistd.h>
41#include <stddef.h>
42#include <string.h>
43#include <ctype.h>
44#include <errno.h>
45#include <syslog.h>
46#include <netdb.h>
47#include <math.h>
48
49#include <altq/altq.h>
50#include <altq/altq_hfsc.h>
51#include "altq_qop.h"
52#include "qop_hfsc.h"
53
54static int read_sc(int *, char ***, int *, u_int *, u_int *, u_int *);
55static int qop_hfsc_enable_hook(struct ifinfo *);
56static int qop_hfsc_delete_class_hook(struct classinfo *);
57static int validate_sc(struct service_curve *);
58
59static void gsc_add_sc(struct gen_sc *, struct service_curve *);
60static void gsc_sub_sc(struct gen_sc *, struct service_curve *);
61static int is_gsc_under_sc(struct gen_sc *, struct service_curve *);
62static void gsc_destroy(struct gen_sc *);
63static struct segment *gsc_getentry(struct gen_sc *, double);
64static int gsc_add_seg(struct gen_sc *, double, double, double, double);
65static int gsc_sub_seg(struct gen_sc *, double, double, double, double);
66static void gsc_compress(struct gen_sc *);
67static double sc_x2y(struct service_curve *, double);
68
69static int hfsc_attach(struct ifinfo *);
70static int hfsc_detach(struct ifinfo *);
71static int hfsc_clear(struct ifinfo *);
72static int hfsc_enable(struct ifinfo *);
73static int hfsc_disable(struct ifinfo *);
74static int hfsc_add_class(struct classinfo *);
75static int hfsc_modify_class(struct classinfo *, void *);
76static int hfsc_delete_class(struct classinfo *);
77static int hfsc_add_filter(struct fltrinfo *);
78static int hfsc_delete_filter(struct fltrinfo *);
79
80#define HFSC_DEVICE	"/dev/altq/hfsc"
81
82static int hfsc_fd = -1;
83static int hfsc_refcount = 0;
84
85static struct qdisc_ops hfsc_qdisc = {
86	ALTQT_HFSC,
87	"hfsc",
88	hfsc_attach,
89	hfsc_detach,
90	hfsc_clear,
91	hfsc_enable,
92	hfsc_disable,
93	hfsc_add_class,
94	hfsc_modify_class,
95	hfsc_delete_class,
96	hfsc_add_filter,
97	hfsc_delete_filter,
98};
99
100#define EQUAL(s1, s2)	(strcmp((s1), (s2)) == 0)
101
102/*
103 * parser interface
104 */
105int
106hfsc_interface_parser(const char *ifname, int argc, char **argv)
107{
108	u_int  	bandwidth = 100000000;	/* 100Mbps */
109	u_int	tbrsize = 0;
110	int	flags = 0;
111
112	/*
113	 * process options
114	 */
115	while (argc > 0) {
116		if (EQUAL(*argv, "bandwidth")) {
117			argc--; argv++;
118			if (argc > 0)
119				bandwidth = atobps(*argv);
120		} else if (EQUAL(*argv, "tbrsize")) {
121			argc--; argv++;
122			if (argc > 0)
123				tbrsize = atobytes(*argv);
124		} else if (EQUAL(*argv, "hfsc")) {
125			/* just skip */
126		} else {
127			LOG(LOG_ERR, 0, "Unknown keyword '%s'", *argv);
128			return (0);
129		}
130		argc--; argv++;
131	}
132
133	if (qcmd_tbr_register(ifname, bandwidth, tbrsize) != 0)
134		return (0);
135
136	if (qcmd_hfsc_add_if(ifname, bandwidth, flags) != 0)
137		return (0);
138	return (1);
139}
140
141int
142hfsc_class_parser(const char *ifname, const char *class_name,
143		  const char *parent_name, int argc, char **argv)
144{
145	u_int	m1, d, m2, rm1, rd, rm2, fm1, fd, fm2, um1, ud, um2;
146	int	qlimit = 50;
147	int	flags = 0, admission = 0;
148	int	type = 0, error;
149
150	rm1 = rd = rm2 = fm1 = fd = fm2 = um1 = ud = um2 = 0;
151	while (argc > 0) {
152		if (*argv[0] == '[') {
153			if (read_sc(&argc, &argv, &type, &m1, &d, &m2) != 0) {
154				LOG(LOG_ERR, 0,
155				    "Bad service curve in %s, line %d",
156				    altqconfigfile, line_no);
157				return (0);
158			}
159			if (type & HFSC_REALTIMESC) {
160				rm1 = m1; rd = d; rm2 = m2;
161			}
162			if (type & HFSC_LINKSHARINGSC) {
163				fm1 = m1; fd = d; fm2 = m2;
164			}
165			if (type & HFSC_UPPERLIMITSC) {
166				um1 = m1; ud = d; um2 = m2;
167			}
168		} else if (EQUAL(*argv, "ulimit")) {
169			argc--; argv++;
170			if (argc > 0) {
171				um2 = atobps(*argv);
172				type |= HFSC_UPPERLIMITSC;
173			}
174		} else if (EQUAL(*argv, "pshare")) {
175			argc--; argv++;
176			if (argc > 0) {
177				struct ifinfo	*ifinfo;
178				u_int pshare;
179
180				pshare = (u_int)strtoul(*argv, NULL, 0);
181				if ((ifinfo = ifname2ifinfo(ifname)) != NULL) {
182					fm2 = ifinfo->bandwidth / 100 * pshare;
183					type |= HFSC_LINKSHARINGSC;
184				}
185			}
186		} else if (EQUAL(*argv, "grate")) {
187			argc--; argv++;
188			if (argc > 0) {
189				rm2 = atobps(*argv);
190				type |= HFSC_REALTIMESC;
191			}
192		} else if (EQUAL(*argv, "bandwidth")) {
193			argc--; argv++;
194			if (argc > 0) {
195				rm2 = fm2 = atobps(*argv);
196				type |= (HFSC_REALTIMESC | HFSC_LINKSHARINGSC);
197			}
198		} else if (EQUAL(*argv, "qlimit")) {
199			argc--; argv++;
200			if (argc > 0)
201				qlimit = strtoul(*argv, NULL, 0);
202		} else if (EQUAL(*argv, "default")) {
203			flags |= HFCF_DEFAULTCLASS;
204		} else if (EQUAL(*argv, "admission")) {
205			argc--; argv++;
206			if (argc > 0) {
207				if (EQUAL(*argv, "guaranteed")
208				    || EQUAL(*argv, "cntlload"))
209					admission = 1;
210				else if (EQUAL(*argv, "none")) {
211					/* nothing */
212				} else {
213					LOG(LOG_ERR, 0,
214					    "unknown admission type - %s, line %d",
215					    *argv, line_no);
216					return (0);
217				}
218			}
219		} else if (EQUAL(*argv, "red")) {
220			flags |= HFCF_RED;
221		} else if (EQUAL(*argv, "ecn")) {
222			flags |= HFCF_ECN;
223		} else if (EQUAL(*argv, "rio")) {
224			flags |= HFCF_RIO;
225		} else if (EQUAL(*argv, "cleardscp")) {
226			flags |= HFCF_CLEARDSCP;
227		} else {
228			LOG(LOG_ERR, 0,
229			    "Unknown keyword '%s' in %s, line %d",
230			    *argv, altqconfigfile, line_no);
231			return (0);
232		}
233
234		argc--; argv++;
235	}
236
237	if (type == 0) {
238		LOG(LOG_ERR, 0,
239		    "hfsc: service curve not specified in %s, line %d",
240		    altqconfigfile, line_no);
241		return (0);
242	}
243
244	if ((flags & HFCF_ECN) && (flags & (HFCF_RED|HFCF_RIO)) == 0)
245		flags |= HFCF_RED;
246
247	/*
248	 * if the link-sharing service curve is diffrent from
249	 * the real-time service curve, we first create a class with the
250	 * smaller service curve and then modify the other service curve.
251	 */
252	if (rm2 <= fm2) {
253		m1 = rm1; d = rd; m2 = rm2;
254	} else {
255		m1 = fm1; d = fd; m2 = fm2;
256	}
257	error = qcmd_hfsc_add_class(ifname, class_name, parent_name,
258				    m1, d, m2, qlimit, flags);
259
260	if (error == 0 && (rm1 != fm1 || rd != fd || rm2 != fm2)) {
261		if (rm2 <= fm2) {
262			m1 = fm1; d = fd; m2 = fm2; type = HFSC_LINKSHARINGSC;
263		} else {
264			m1 = rm1; d = rd; m2 = rm2; type = HFSC_REALTIMESC;
265		}
266		error = qcmd_hfsc_modify_class(ifname, class_name,
267					       m1, d, m2, type);
268	}
269
270	if (error == 0 && (um1 != 0 || um2 != 0)) {
271		error = qcmd_hfsc_modify_class(ifname, class_name,
272		    um1, ud, um2, HFSC_UPPERLIMITSC);
273	}
274
275	if (error == 0 && admission) {
276		/* this is a special class for rsvp */
277		struct ifinfo *ifinfo = ifname2ifinfo(ifname);
278		struct classinfo *clinfo = clname2clinfo(ifinfo, class_name);
279
280		if (ifinfo->resv_class != NULL) {
281			LOG(LOG_ERR, 0,
282			    "more than one admission class specified: %s",
283			    class_name);
284			return (0);
285		}
286		ifinfo->resv_class = clinfo;
287	}
288
289	if (error) {
290		LOG(LOG_ERR, errno, "hfsc_class_parser: %s",
291		    qoperror(error));
292		return (0);
293	}
294	return (1);
295}
296
297/*
298 * read service curve parameters
299 * '[' <type> <m1> <d> <m2> ']'
300 *  type := "sc", "rt", "ls", or "ul"
301 */
302static int
303read_sc(int *argcp, char ***argvp, int *type, u_int *m1, u_int *d, u_int *m2)
304{
305	int argc = *argcp;
306	char **argv = *argvp;
307	char *cp;
308
309	cp = *argv;
310	if (*cp++ != '[')
311		return (-1);
312	if (*cp == '\0') {
313		cp = *++argv; --argc;
314	}
315	if (*cp == 's' || *cp == 'S')
316		*type = HFSC_DEFAULTSC;
317	else if (*cp == 'r' || *cp == 'R')
318		*type = HFSC_REALTIMESC;
319	else if (*cp == 'l' || *cp == 'L')
320		*type = HFSC_LINKSHARINGSC;
321	else if (*cp == 'u' || *cp == 'U')
322		*type = HFSC_UPPERLIMITSC;
323	else
324		return (-1);
325	cp = *++argv; --argc;
326	*m1 = atobps(cp);
327	cp = *++argv; --argc;
328	*d = (u_int)strtoul(cp, NULL, 0);
329	cp = *++argv; --argc;
330	*m2 = atobps(cp);
331	if (strchr(cp, ']') == NULL) {
332		cp = *++argv; --argc;
333		if (*cp != ']')
334			return (-1);
335	}
336	*argcp = argc;
337	*argvp = argv;
338	return (0);
339}
340
341/*
342 * qcmd api
343 */
344int
345qcmd_hfsc_add_if(const char *ifname, u_int bandwidth, int flags)
346{
347	int error;
348
349	error = qop_hfsc_add_if(NULL, ifname, bandwidth, flags);
350	if (error != 0)
351		LOG(LOG_ERR, errno, "%s: can't add hfsc on interface '%s'",
352		    qoperror(error), ifname);
353	return (error);
354}
355
356int
357qcmd_hfsc_add_class(const char *ifname, const char *class_name,
358		    const char *parent_name, u_int m1, u_int d, u_int m2,
359		    int qlimit, int flags)
360{
361	struct ifinfo *ifinfo;
362	struct classinfo *parent = NULL;
363	struct service_curve sc;
364	int error = 0;
365
366	if ((ifinfo = ifname2ifinfo(ifname)) == NULL)
367		error = QOPERR_BADIF;
368
369	if (error == 0 &&
370	    (parent = clname2clinfo(ifinfo, parent_name)) == NULL)
371		error = QOPERR_BADCLASS;
372
373	sc.m1 = m1;
374	sc.d = d;
375	sc.m2 = m2;
376
377	if (error == 0)
378		error = qop_hfsc_add_class(NULL, class_name, ifinfo, parent,
379					   &sc, qlimit, flags);
380	if (error != 0)
381		LOG(LOG_ERR, errno,
382		    "hfsc: %s: can't add class '%s' on interface '%s'",
383		    qoperror(error), class_name, ifname);
384	return (error);
385}
386
387int
388qcmd_hfsc_modify_class(const char *ifname, const char *class_name,
389		       u_int m1, u_int d, u_int m2, int sctype)
390{
391	struct ifinfo *ifinfo;
392	struct classinfo *clinfo;
393	struct service_curve sc;
394
395	if ((ifinfo = ifname2ifinfo(ifname)) == NULL)
396		return (QOPERR_BADIF);
397
398	if ((clinfo = clname2clinfo(ifinfo, class_name)) == NULL)
399		return (QOPERR_BADCLASS);
400
401	sc.m1 = m1;
402	sc.d = d;
403	sc.m2 = m2;
404
405	return qop_hfsc_modify_class(clinfo, &sc, sctype);
406}
407
408/*
409 * qop api
410 */
411int
412qop_hfsc_add_if(struct ifinfo **rp, const char *ifname,
413		u_int bandwidth, int flags)
414{
415	struct ifinfo *ifinfo = NULL;
416	struct hfsc_ifinfo *hfsc_ifinfo = NULL;
417	struct service_curve sc;
418	int error;
419
420	if ((hfsc_ifinfo = calloc(1, sizeof(*hfsc_ifinfo))) == NULL)
421		return (QOPERR_NOMEM);
422
423	error = qop_add_if(&ifinfo, ifname, bandwidth,
424			   &hfsc_qdisc, hfsc_ifinfo);
425	if (error != 0)
426		goto err_ret;
427
428	/* set enable hook */
429	ifinfo->enable_hook = qop_hfsc_enable_hook;
430
431	/* create root class */
432	sc.m1 = bandwidth;
433	sc.d = 0;
434	sc.m2 = bandwidth;
435	if ((error = qop_hfsc_add_class(&hfsc_ifinfo->root_class, "root",
436					ifinfo, NULL, &sc, 0, 0)) != 0) {
437		LOG(LOG_ERR, errno,
438		    "hfsc: %s: can't create dummy root class on %s!",
439		    qoperror(error), ifname);
440		(void)qop_delete_if(ifinfo);
441		return (QOPERR_CLASS);
442	}
443
444	if (rp != NULL)
445		*rp = ifinfo;
446	return (0);
447
448 err_ret:
449	if (hfsc_ifinfo != NULL) {
450		free(hfsc_ifinfo);
451		if (ifinfo != NULL)
452			ifinfo->private = NULL;
453	}
454	return (error);
455}
456
457#define is_sc_null(sc)	(((sc) == NULL) || ((sc)->m1 == 0 && (sc)->m2 == 0))
458
459int
460qop_hfsc_add_class(struct classinfo **rp, const char *class_name,
461		   struct ifinfo *ifinfo, struct classinfo *parent,
462		   struct service_curve *sc, int qlimit, int flags)
463{
464	struct classinfo *clinfo;
465	struct hfsc_ifinfo *hfsc_ifinfo;
466	struct hfsc_classinfo *hfsc_clinfo = NULL, *parent_clinfo = NULL;
467	int error;
468
469	hfsc_ifinfo = ifinfo->private;
470	if ((flags & HFCF_DEFAULTCLASS) && hfsc_ifinfo->default_class != NULL)
471		return (QOPERR_CLASS_INVAL);
472
473	if (validate_sc(sc) != 0)
474		return (QOPERR_INVAL);
475
476	/* admission control */
477	if (parent != NULL && !is_sc_null(sc)) {
478		parent_clinfo = parent->private;
479		gsc_add_sc(&parent_clinfo->gen_rsc, sc);
480		gsc_add_sc(&parent_clinfo->gen_fsc, sc);
481		if (!is_gsc_under_sc(&parent_clinfo->gen_rsc,
482				     &parent_clinfo->rsc) ||
483		    !is_gsc_under_sc(&parent_clinfo->gen_fsc,
484				     &parent_clinfo->fsc)) {
485			/* admission control failure */
486			error = QOPERR_ADMISSION_NOBW;
487			goto err_ret;
488		}
489	}
490
491	if ((hfsc_clinfo = calloc(1, sizeof(*hfsc_clinfo))) == NULL) {
492		error = QOPERR_NOMEM;
493		goto err_ret;
494	}
495
496	hfsc_clinfo->rsc = *sc;
497	hfsc_clinfo->fsc = *sc;
498	LIST_INIT(&hfsc_clinfo->gen_rsc);
499	LIST_INIT(&hfsc_clinfo->gen_fsc);
500	hfsc_clinfo->qlimit = qlimit;
501	hfsc_clinfo->flags = flags;
502
503	if ((error = qop_add_class(&clinfo, class_name, ifinfo, parent,
504				   hfsc_clinfo)) != 0)
505		goto err_ret;
506
507	/* set delete hook */
508	clinfo->delete_hook = qop_hfsc_delete_class_hook;
509
510	if (flags & HFCF_DEFAULTCLASS)
511		hfsc_ifinfo->default_class = clinfo;
512
513	if (parent == NULL) {
514		/*
515		 * if this is a root class, reserve 20% of the real-time
516		 * bandwidth for safety.
517		 * many network cards are not able to saturate the wire,
518		 * and if we allocate real-time traffic more than the
519		 * maximum sending rate of the card, hfsc is no longer
520		 * able to meet the delay bound requirements.
521		 */
522		hfsc_clinfo->rsc.m1 = hfsc_clinfo->rsc.m1 / 10 * 8;
523		hfsc_clinfo->rsc.m2 = hfsc_clinfo->rsc.m2 / 10 * 8;
524	}
525
526	if (rp != NULL)
527		*rp = clinfo;
528	return (0);
529
530 err_ret:
531	/* cancel admission control */
532	if (parent != NULL && !is_sc_null(sc)) {
533		gsc_sub_sc(&parent_clinfo->gen_rsc, sc);
534		gsc_sub_sc(&parent_clinfo->gen_fsc, sc);
535	}
536
537	if (hfsc_clinfo != NULL) {
538		free(hfsc_clinfo);
539		clinfo->private = NULL;
540	}
541
542	return (error);
543}
544
545/*
546 * this is called from qop_delete_class() before a class is destroyed
547 * for discipline specific cleanup.
548 */
549static int
550qop_hfsc_delete_class_hook(struct classinfo *clinfo)
551{
552	struct hfsc_classinfo *hfsc_clinfo, *parent_clinfo;
553
554	hfsc_clinfo = clinfo->private;
555
556	/* cancel admission control */
557	if (clinfo->parent != NULL) {
558		parent_clinfo = clinfo->parent->private;
559
560		gsc_sub_sc(&parent_clinfo->gen_rsc, &hfsc_clinfo->rsc);
561		gsc_sub_sc(&parent_clinfo->gen_fsc, &hfsc_clinfo->fsc);
562	}
563
564	gsc_destroy(&hfsc_clinfo->gen_rsc);
565	gsc_destroy(&hfsc_clinfo->gen_fsc);
566	return (0);
567}
568
569int
570qop_hfsc_modify_class(struct classinfo *clinfo,
571		      struct service_curve *sc, int sctype)
572{
573	struct hfsc_classinfo *hfsc_clinfo, *parent_clinfo;
574	struct service_curve rsc, fsc, usc;
575	int error;
576
577	if (validate_sc(sc) != 0)
578		return (QOPERR_INVAL);
579
580	hfsc_clinfo = clinfo->private;
581	if (clinfo->parent == NULL)
582		return (QOPERR_CLASS_INVAL);
583	parent_clinfo = clinfo->parent->private;
584
585	/* save old service curves */
586	rsc = hfsc_clinfo->rsc;
587	fsc = hfsc_clinfo->fsc;
588	usc = hfsc_clinfo->usc;
589
590	/* admission control */
591	if (sctype & HFSC_REALTIMESC) {
592		/* if the class has usc, rsc should be smaller than usc */
593		if (!is_sc_null(&hfsc_clinfo->usc)) {
594			gsc_head_t tmp_gen_rsc =
595			    LIST_HEAD_INITIALIZER(tmp_gen_rsc);
596
597			gsc_add_sc(&tmp_gen_rsc, sc);
598			if (!is_gsc_under_sc(&tmp_gen_rsc, &hfsc_clinfo->usc)) {
599				gsc_destroy(&tmp_gen_rsc);
600				return (QOPERR_ADMISSION);
601			}
602			gsc_destroy(&tmp_gen_rsc);
603		}
604
605		if (!is_gsc_under_sc(&hfsc_clinfo->gen_rsc, sc)) {
606			/* admission control failure */
607			return (QOPERR_ADMISSION);
608		}
609
610		gsc_sub_sc(&parent_clinfo->gen_rsc, &hfsc_clinfo->rsc);
611		gsc_add_sc(&parent_clinfo->gen_rsc, sc);
612		if (!is_gsc_under_sc(&parent_clinfo->gen_rsc,
613				     &parent_clinfo->rsc)) {
614			/* admission control failure */
615			gsc_sub_sc(&parent_clinfo->gen_rsc, sc);
616			gsc_add_sc(&parent_clinfo->gen_rsc, &hfsc_clinfo->rsc);
617			return (QOPERR_ADMISSION_NOBW);
618		}
619		hfsc_clinfo->rsc = *sc;
620	}
621	if (sctype & HFSC_LINKSHARINGSC) {
622		if (!is_gsc_under_sc(&hfsc_clinfo->gen_fsc, sc)) {
623			/* admission control failure */
624			return (QOPERR_ADMISSION);
625		}
626
627		gsc_sub_sc(&parent_clinfo->gen_fsc, &hfsc_clinfo->fsc);
628		gsc_add_sc(&parent_clinfo->gen_fsc, sc);
629		if (!is_gsc_under_sc(&parent_clinfo->gen_fsc,
630				     &parent_clinfo->fsc)) {
631			/* admission control failure */
632			gsc_sub_sc(&parent_clinfo->gen_fsc, sc);
633			gsc_add_sc(&parent_clinfo->gen_fsc, &hfsc_clinfo->fsc);
634			return (QOPERR_ADMISSION_NOBW);
635		}
636		hfsc_clinfo->fsc = *sc;
637	}
638	if (sctype & HFSC_UPPERLIMITSC) {
639		if (!is_sc_null(sc)) {
640			/* usc must be smaller than interface bandwidth */
641			struct classinfo *root_clinfo =
642			    clname2clinfo(clinfo->ifinfo, "root");
643			if (root_clinfo != NULL) {
644				struct hfsc_classinfo *root_hfsc_clinfo =
645				    root_clinfo->private;
646				if (!is_sc_null(&root_hfsc_clinfo->rsc)) {
647					gsc_head_t tmp_gen_usc =
648					    LIST_HEAD_INITIALIZER(tmp_gen_usc);
649					gsc_add_sc(&tmp_gen_usc, sc);
650					if (!is_gsc_under_sc(&tmp_gen_usc,
651					    &root_hfsc_clinfo->fsc)) {
652						/* illegal attempt to set
653						   upper limit curve to be
654						   greater than the interface
655						   bandwidth */
656						gsc_destroy(&tmp_gen_usc);
657						return (QOPERR_ADMISSION);
658					}
659					gsc_destroy(&tmp_gen_usc);
660				}
661			}
662			/* if this class has rsc, check that usc >= rsc */
663			if (!is_sc_null(&hfsc_clinfo->rsc)) {
664				gsc_head_t tmp_gen_rsc =
665				    LIST_HEAD_INITIALIZER(tmp_gen_rsc);
666				gsc_add_sc(&tmp_gen_rsc, &hfsc_clinfo->rsc);
667				if (!is_gsc_under_sc(&tmp_gen_rsc, sc)) {
668					/* illegal attempt to set upper limit
669					   curve to be under the real-time
670					   service curve */
671					gsc_destroy(&tmp_gen_rsc);
672					return (QOPERR_ADMISSION);
673				}
674				gsc_destroy(&tmp_gen_rsc);
675			}
676		}
677		hfsc_clinfo->usc = *sc;
678	}
679
680	error = qop_modify_class(clinfo, (void *)((long)sctype));
681	if (error == 0)
682		return (0);
683
684	/* modify failed!, restore the old service curves */
685	if (sctype & HFSC_REALTIMESC) {
686		gsc_sub_sc(&parent_clinfo->gen_rsc, sc);
687		gsc_add_sc(&parent_clinfo->gen_rsc, &rsc);
688		hfsc_clinfo->rsc = rsc;
689	}
690	if (sctype & HFSC_LINKSHARINGSC) {
691		gsc_sub_sc(&parent_clinfo->gen_fsc, sc);
692		gsc_add_sc(&parent_clinfo->gen_fsc, &fsc);
693		hfsc_clinfo->fsc = fsc;
694	}
695	if (sctype & HFSC_UPPERLIMITSC) {
696		hfsc_clinfo->usc = usc;
697	}
698	return (error);
699}
700
701/*
702 * sanity check at enabling hfsc:
703 *  1. there must one default class for an interface
704 *  2. the default class must be a leaf class
705 *  3. an internal class should not have filters
706 * (rule 2 and 3 are due to the fact that the hfsc link-sharing algorithm
707 *  do not schedule internal classes.)
708 */
709static int
710qop_hfsc_enable_hook(struct ifinfo *ifinfo)
711{
712	struct hfsc_ifinfo *hfsc_ifinfo;
713	struct classinfo *clinfo;
714
715	hfsc_ifinfo = ifinfo->private;
716	if (hfsc_ifinfo->default_class == NULL) {
717		LOG(LOG_ERR, 0, "hfsc: no default class on interface %s!",
718		    ifinfo->ifname);
719		return (QOPERR_CLASS);
720	} else if (hfsc_ifinfo->default_class->child != NULL) {
721		LOG(LOG_ERR, 0, "hfsc: default class on %s must be a leaf!",
722		    ifinfo->ifname);
723		return (QOPERR_CLASS);
724	}
725
726	LIST_FOREACH(clinfo, &ifinfo->cllist, next) {
727		if (clinfo->child != NULL && !LIST_EMPTY(&clinfo->fltrlist)) {
728			LOG(LOG_ERR, 0,
729			    "hfsc: internal class \"%s\" should not have a filter!",
730			    clinfo->clname);
731			return (QOPERR_CLASS);
732		}
733	}
734
735	return (0);
736}
737
738static int
739validate_sc(struct service_curve *sc)
740{
741	/* the 1st segment of a concave curve must be zero */
742	if (sc->m1 < sc->m2 && sc->m1 != 0) {
743		LOG(LOG_ERR, 0, "m1 must be 0 for convex!");
744		return (-1);
745	}
746	if (sc->m1 > sc->m2 && sc->m2 == 0) {
747		LOG(LOG_ERR, 0, "m2 must be nonzero for concave!");
748		return (-1);
749	}
750	return (0);
751}
752
753/*
754 * admission control using generalized service curve
755 */
756
757/* add a new service curve to a generilized service curve */
758static void
759gsc_add_sc(struct gen_sc *gsc, struct service_curve *sc)
760{
761	if (is_sc_null(sc))
762		return;
763	if (sc->d != 0)
764		gsc_add_seg(gsc, 0, 0, (double)sc->d, (double)sc->m1);
765	gsc_add_seg(gsc, (double)sc->d, 0, HUGE_VAL, (double)sc->m2);
766}
767
768/* subtract a service curve from a generilized service curve */
769static void
770gsc_sub_sc(struct gen_sc *gsc, struct service_curve *sc)
771{
772	if (is_sc_null(sc))
773		return;
774	if (sc->d != 0)
775		gsc_sub_seg(gsc, 0, 0, (double)sc->d, (double)sc->m1);
776	gsc_sub_seg(gsc, (double)sc->d, 0, HUGE_VAL, (double)sc->m2);
777}
778
779/*
780 * check whether all points of a generalized service curve have
781 * their y-coordinates no larger than a given two-piece linear
782 * service curve.
783 */
784static int
785is_gsc_under_sc(struct gen_sc *gsc, struct service_curve *sc)
786{
787	struct segment *s, *last, *end;
788	double y;
789
790	if (is_sc_null(sc)) {
791		if (LIST_EMPTY(gsc))
792			return (1);
793		LIST_FOREACH(s, gsc, _next) {
794			if (s->m != 0)
795				return (0);
796		}
797		return (1);
798	}
799	/*
800	 * gsc has a dummy entry at the end with x = HUGE_VAL.
801	 * loop through up to this dummy entry.
802	 */
803	end = gsc_getentry(gsc, HUGE_VAL);
804	if (end == NULL)
805		return (1);
806	last = NULL;
807	for (s = LIST_FIRST(gsc); s != end; s = LIST_NEXT(s, _next)) {
808		if (s->y > sc_x2y(sc, s->x))
809			return (0);
810		last = s;
811	}
812	/* last now holds the real last segment */
813	if (last == NULL)
814		return (1);
815	if (last->m > sc->m2)
816		return (0);
817	if (last->x < sc->d && last->m > sc->m1) {
818		y = last->y + (sc->d - last->x) * last->m;
819		if (y > sc_x2y(sc, sc->d))
820			return (0);
821	}
822	return (1);
823}
824
825static void
826gsc_destroy(struct gen_sc *gsc)
827{
828	struct segment *s;
829
830	while ((s = LIST_FIRST(gsc)) != NULL) {
831		LIST_REMOVE(s, _next);
832		free(s);
833	}
834}
835
836/*
837 * return a segment entry starting at x.
838 * if gsc has no entry starting at x, a new entry is created at x.
839 */
840static struct segment *
841gsc_getentry(struct gen_sc *gsc, double x)
842{
843	struct segment *new, *prev, *s;
844
845	prev = NULL;
846	LIST_FOREACH(s, gsc, _next) {
847		if (s->x == x)
848			return (s);	/* matching entry found */
849		else if (s->x < x)
850			prev = s;
851		else
852			break;
853	}
854
855	/* we have to create a new entry */
856	if ((new = calloc(1, sizeof(struct segment))) == NULL)
857		return (NULL);
858
859	new->x = x;
860	if (x == HUGE_VAL || s == NULL)
861		new->d = 0;
862	else if (s->x == HUGE_VAL)
863		new->d = HUGE_VAL;
864	else
865		new->d = s->x - x;
866	if (prev == NULL) {
867		/* insert the new entry at the head of the list */
868		new->y = 0;
869		new->m = 0;
870		LIST_INSERT_HEAD(gsc, new, _next);
871	} else {
872		/*
873		 * the start point intersects with the segment pointed by
874		 * prev.  divide prev into 2 segments
875		 */
876		if (x == HUGE_VAL) {
877			prev->d = HUGE_VAL;
878			if (prev->m == 0)
879				new->y = prev->y;
880			else
881				new->y = HUGE_VAL;
882		} else {
883			prev->d = x - prev->x;
884			new->y = prev->d * prev->m + prev->y;
885		}
886		new->m = prev->m;
887		LIST_INSERT_AFTER(prev, new, _next);
888	}
889	return (new);
890}
891
892/* add a segment to a generalized service curve */
893static int
894gsc_add_seg(struct gen_sc *gsc, double x, double y, double d, double m)
895{
896	struct segment *start, *end, *s;
897	double x2;
898
899	if (d == HUGE_VAL)
900		x2 = HUGE_VAL;
901	else
902		x2 = x + d;
903	start = gsc_getentry(gsc, x);
904	end   = gsc_getentry(gsc, x2);
905	if (start == NULL || end == NULL)
906		return (-1);
907
908	for (s = start; s != end; s = LIST_NEXT(s, _next)) {
909		s->m += m;
910		s->y += y + (s->x - x) * m;
911	}
912
913	end = gsc_getentry(gsc, HUGE_VAL);
914	for (; s != end; s = LIST_NEXT(s, _next)) {
915		s->y += m * d;
916	}
917
918	return (0);
919}
920
921/* subtract a segment from a generalized service curve */
922static int
923gsc_sub_seg(struct gen_sc *gsc, double x, double y, double d, double m)
924{
925	if (gsc_add_seg(gsc, x, y, d, -m) < 0)
926		return (-1);
927	gsc_compress(gsc);
928	return (0);
929}
930
931/*
932 * collapse adjacent segments with the same slope
933 */
934static void
935gsc_compress(struct gen_sc *gsc)
936{
937	struct segment *s, *next;
938
939 again:
940	LIST_FOREACH(s, gsc, _next) {
941
942		if ((next = LIST_NEXT(s, _next)) == NULL) {
943			if (LIST_FIRST(gsc) == s && s->m == 0) {
944				/*
945				 * if this is the only entry and its
946				 * slope is 0, it's a remaining dummy
947				 * entry. we can discard it.
948				 */
949				LIST_REMOVE(s, _next);
950				free(s);
951			}
952			break;
953		}
954
955		if (s->x == next->x) {
956			/* discard this entry */
957			LIST_REMOVE(s, _next);
958			free(s);
959			goto again;
960		} else if (s->m == next->m) {
961			/* join the two entries */
962			if (s->d != HUGE_VAL && next->d != HUGE_VAL)
963				s->d += next->d;
964			LIST_REMOVE(next, _next);
965			free(next);
966			goto again;
967		}
968	}
969}
970
971/* get y-projection of a service curve */
972static double
973sc_x2y(struct service_curve *sc, double x)
974{
975	double y;
976
977	if (x <= (double)sc->d)
978		/* y belongs to the 1st segment */
979		y = x * (double)sc->m1;
980	else
981		/* y belongs to the 2nd segment */
982		y = (double)sc->d * (double)sc->m1
983			+ (x - (double)sc->d) * (double)sc->m2;
984	return (y);
985}
986
987/*
988 *  system call interfaces for qdisc_ops
989 */
990static int
991hfsc_attach(struct ifinfo *ifinfo)
992{
993	struct hfsc_attach attach;
994
995	if (hfsc_fd < 0 &&
996	    (hfsc_fd = open(HFSC_DEVICE, O_RDWR)) < 0 &&
997	    (hfsc_fd = open_module(HFSC_DEVICE, O_RDWR)) < 0) {
998		LOG(LOG_ERR, errno, "HFSC open");
999		return (QOPERR_SYSCALL);
1000	}
1001
1002	hfsc_refcount++;
1003	memset(&attach, 0, sizeof(attach));
1004	strncpy(attach.iface.hfsc_ifname, ifinfo->ifname, IFNAMSIZ);
1005	attach.bandwidth = ifinfo->bandwidth;
1006
1007	if (ioctl(hfsc_fd, HFSC_IF_ATTACH, &attach) < 0)
1008		return (QOPERR_SYSCALL);
1009	return (0);
1010}
1011
1012static int
1013hfsc_detach(struct ifinfo *ifinfo)
1014{
1015	struct hfsc_interface iface;
1016
1017	memset(&iface, 0, sizeof(iface));
1018	strncpy(iface.hfsc_ifname, ifinfo->ifname, IFNAMSIZ);
1019
1020	if (ioctl(hfsc_fd, HFSC_IF_DETACH, &iface) < 0)
1021		return (QOPERR_SYSCALL);
1022
1023	if (--hfsc_refcount == 0) {
1024		close(hfsc_fd);
1025		hfsc_fd = -1;
1026	}
1027	return (0);
1028}
1029
1030static int
1031hfsc_clear(struct ifinfo *ifinfo)
1032{
1033	struct hfsc_interface iface;
1034
1035	memset(&iface, 0, sizeof(iface));
1036	strncpy(iface.hfsc_ifname, ifinfo->ifname, IFNAMSIZ);
1037
1038	if (ioctl(hfsc_fd, HFSC_CLEAR_HIERARCHY, &iface) < 0)
1039		return (QOPERR_SYSCALL);
1040	return (0);
1041}
1042
1043static int
1044hfsc_enable(struct ifinfo *ifinfo)
1045{
1046	struct hfsc_interface iface;
1047
1048	memset(&iface, 0, sizeof(iface));
1049	strncpy(iface.hfsc_ifname, ifinfo->ifname, IFNAMSIZ);
1050
1051	if (ioctl(hfsc_fd, HFSC_ENABLE, &iface) < 0)
1052		return (QOPERR_SYSCALL);
1053	return (0);
1054}
1055
1056static int
1057hfsc_disable(struct ifinfo *ifinfo)
1058{
1059	struct hfsc_interface iface;
1060
1061	memset(&iface, 0, sizeof(iface));
1062	strncpy(iface.hfsc_ifname, ifinfo->ifname, IFNAMSIZ);
1063
1064	if (ioctl(hfsc_fd, HFSC_DISABLE, &iface) < 0)
1065		return (QOPERR_SYSCALL);
1066	return (0);
1067}
1068
1069static int
1070hfsc_add_class(struct classinfo *clinfo)
1071{
1072	struct hfsc_add_class class_add;
1073	struct hfsc_classinfo *hfsc_clinfo;
1074	struct hfsc_ifinfo *hfsc_ifinfo;
1075
1076	hfsc_ifinfo = clinfo->ifinfo->private;
1077	hfsc_clinfo = clinfo->private;
1078
1079	memset(&class_add, 0, sizeof(class_add));
1080	strncpy(class_add.iface.hfsc_ifname, clinfo->ifinfo->ifname, IFNAMSIZ);
1081
1082	if (clinfo->parent == NULL)
1083		class_add.parent_handle = HFSC_NULLCLASS_HANDLE;
1084	else
1085		class_add.parent_handle = clinfo->parent->handle;
1086
1087	class_add.service_curve = hfsc_clinfo->rsc;
1088	class_add.qlimit = hfsc_clinfo->qlimit;
1089	class_add.flags = hfsc_clinfo->flags;
1090
1091	if (ioctl(hfsc_fd, HFSC_ADD_CLASS, &class_add) < 0) {
1092		clinfo->handle = HFSC_NULLCLASS_HANDLE;
1093		return (QOPERR_SYSCALL);
1094	}
1095	clinfo->handle = class_add.class_handle;
1096	return (0);
1097}
1098
1099static int
1100hfsc_modify_class(struct classinfo *clinfo, void *arg)
1101{
1102	struct hfsc_modify_class class_mod;
1103	struct hfsc_classinfo *hfsc_clinfo;
1104	long sctype;
1105
1106	sctype = (long)arg;
1107	hfsc_clinfo = clinfo->private;
1108
1109	memset(&class_mod, 0, sizeof(class_mod));
1110	strncpy(class_mod.iface.hfsc_ifname, clinfo->ifinfo->ifname, IFNAMSIZ);
1111	class_mod.class_handle = clinfo->handle;
1112	if (sctype & HFSC_REALTIMESC)
1113		class_mod.service_curve = hfsc_clinfo->rsc;
1114	else if (sctype & HFSC_LINKSHARINGSC)
1115		class_mod.service_curve = hfsc_clinfo->fsc;
1116	else if (sctype & HFSC_UPPERLIMITSC)
1117		class_mod.service_curve = hfsc_clinfo->usc;
1118	else
1119		return (QOPERR_INVAL);
1120	class_mod.sctype = sctype;
1121
1122	if (ioctl(hfsc_fd, HFSC_MOD_CLASS, &class_mod) < 0)
1123		return (QOPERR_SYSCALL);
1124	return (0);
1125}
1126
1127static int
1128hfsc_delete_class(struct classinfo *clinfo)
1129{
1130	struct hfsc_delete_class class_delete;
1131
1132	if (clinfo->handle == HFSC_NULLCLASS_HANDLE)
1133		return (0);
1134
1135	memset(&class_delete, 0, sizeof(class_delete));
1136	strncpy(class_delete.iface.hfsc_ifname, clinfo->ifinfo->ifname,
1137		IFNAMSIZ);
1138	class_delete.class_handle = clinfo->handle;
1139
1140	if (ioctl(hfsc_fd, HFSC_DEL_CLASS, &class_delete) < 0)
1141		return (QOPERR_SYSCALL);
1142	return (0);
1143}
1144
1145static int
1146hfsc_add_filter(struct fltrinfo *fltrinfo)
1147{
1148	struct hfsc_add_filter fltr_add;
1149
1150	memset(&fltr_add, 0, sizeof(fltr_add));
1151	strncpy(fltr_add.iface.hfsc_ifname, fltrinfo->clinfo->ifinfo->ifname,
1152		IFNAMSIZ);
1153	fltr_add.class_handle = fltrinfo->clinfo->handle;
1154	fltr_add.filter = fltrinfo->fltr;
1155
1156	if (ioctl(hfsc_fd, HFSC_ADD_FILTER, &fltr_add) < 0)
1157		return (QOPERR_SYSCALL);
1158	fltrinfo->handle = fltr_add.filter_handle;
1159	return (0);
1160}
1161
1162static int
1163hfsc_delete_filter(struct fltrinfo *fltrinfo)
1164{
1165	struct hfsc_delete_filter fltr_del;
1166
1167	memset(&fltr_del, 0, sizeof(fltr_del));
1168	strncpy(fltr_del.iface.hfsc_ifname, fltrinfo->clinfo->ifinfo->ifname,
1169		IFNAMSIZ);
1170	fltr_del.filter_handle = fltrinfo->handle;
1171
1172	if (ioctl(hfsc_fd, HFSC_DEL_FILTER, &fltr_del) < 0)
1173		return (QOPERR_SYSCALL);
1174	return (0);
1175}
1176