Merge branch 'hfsc-qdisc-support'

http://lists.infradead.org/pipermail/libnl/2014-July/001566.html
http://lists.infradead.org/pipermail/libnl/2014-July/001594.html

Signed-off-by: Thomas Haller <thaller@redhat.com>

9 files changed, 718 insertions, 38 deletions

diff --git a/include/Makefile.am b/include/Makefile.am
index 90f647c..74bbd5a 100644
--- a/include/Makefile.am
+++ b/include/Makefile.am
@@ -70,6 +70,7 @@ nobase_libnlinclude_HEADERS = \
 	netlink/route/qdisc/tbf.h \
 	netlink/route/qdisc/plug.h \
 	netlink/route/qdisc/fq_codel.h \
+	netlink/route/qdisc/hfsc.h \
 	netlink/route/addr.h \
 	netlink/route/class.h \
 	netlink/route/classifier.h \
diff --git a/include/netlink-private/route/tc-api.h b/include/netlink-private/route/tc-api.h
index bf0c8a3..ce62080 100644
--- a/include/netlink-private/route/tc-api.h
+++ b/include/netlink-private/route/tc-api.h
@@ -116,7 +116,7 @@ extern int			rtnl_tc_compare(struct nl_object *,
 
 extern void *			rtnl_tc_data(struct rtnl_tc *);
 extern void *			rtnl_tc_data_check(struct rtnl_tc *,
-						   struct rtnl_tc_ops *);
+						   struct rtnl_tc_ops *, int *);
 
 extern struct rtnl_tc_ops *	rtnl_tc_lookup_ops(enum rtnl_tc_type,
 						   const char *);
diff --git a/include/netlink-private/types.h b/include/netlink-private/types.h
index 10bdaaa..6f3243b 100644
--- a/include/netlink-private/types.h
+++ b/include/netlink-private/types.h
@@ -721,6 +721,20 @@ struct rtnl_fq_codel
 	uint32_t	fq_mask;
 };
 
+struct rtnl_hfsc_qdisc
+{
+	uint32_t		qh_defcls;
+	uint32_t		qh_mask;
+};
+
+struct rtnl_hfsc_class
+{
+	struct tc_service_curve ch_rsc;
+	struct tc_service_curve ch_fsc;
+	struct tc_service_curve ch_usc;
+	uint32_t		ch_mask;
+};
+
 struct flnl_request
 {
 	NLHDR_COMMON
diff --git a/include/netlink/route/qdisc/hfsc.h b/include/netlink/route/qdisc/hfsc.h
new file mode 100644
index 0000000..8d34fe5
--- /dev/null
+++ b/include/netlink/route/qdisc/hfsc.h
@@ -0,0 +1,37 @@
+/*
+ * netlink/route/sch/hfsc.h	HFSC Qdisc
+ *
+ *	This library is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation version 2.1
+ *	of the License.
+ *
+ * Copyright (c) 2014 Cong Wang <xiyou.wangcong@gmail.com>
+ */
+
+#ifndef NETLINK_HFSC_H_
+#define NETLINK_HFSC_H_
+
+#include <netlink/netlink.h>
+#include <netlink/route/tc.h>
+#include <netlink/route/qdisc.h>
+#include <netlink/route/class.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern uint32_t	rtnl_qdisc_hfsc_get_defcls(const struct rtnl_qdisc *);
+extern int	rtnl_qdisc_hfsc_set_defcls(struct rtnl_qdisc *, uint32_t);
+
+extern int rtnl_class_hfsc_get_rsc(const struct rtnl_class *class, struct tc_service_curve *tsc);
+extern int rtnl_class_hfsc_set_rsc(struct rtnl_class *class, const struct tc_service_curve *tsc);
+extern int rtnl_class_hfsc_get_fsc(const struct rtnl_class *class, struct tc_service_curve *tsc);
+extern int rtnl_class_hfsc_set_fsc(struct rtnl_class *class, const struct tc_service_curve *tsc);
+extern int rtnl_class_hfsc_get_usc(const struct rtnl_class *class, struct tc_service_curve *tsc);
+extern int rtnl_class_hfsc_set_usc(struct rtnl_class *class, const struct tc_service_curve *tsc);
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/lib/Makefile.am b/lib/Makefile.am
index ee9c00d..2567323 100644
--- a/lib/Makefile.am
+++ b/lib/Makefile.am
@@ -86,7 +86,7 @@ libnl_route_3_la_SOURCES = \
 	route/qdisc/fifo.c route/qdisc/htb.c route/qdisc/netem.c \
 	route/qdisc/prio.c route/qdisc/red.c route/qdisc/sfq.c \
 	route/qdisc/tbf.c route/qdisc/plug.c route/qdisc/ingress.c \
-	route/qdisc/fq_codel.c \
+	route/qdisc/fq_codel.c route/qdisc/hfsc.c \
 	\
 	fib_lookup/lookup.c fib_lookup/request.c \
 	\
@@ -128,6 +128,7 @@ nobase_pkglib_LTLIBRARIES = \
 	cli/qdisc/bfifo.la \
 	cli/qdisc/ingress.la \
 	cli/qdisc/fq_codel.la \
+	cli/qdisc/hfsc.la \
 	cli/cls/basic.la \
 	cli/cls/cgroup.la
 
@@ -138,6 +139,7 @@ cli_qdisc_plug_la_LDFLAGS = -module -avoid-version
 cli_qdisc_bfifo_la_LDFLAGS = -module -avoid-version
 cli_qdisc_ingress_la_LDFLAGS = -module -avoid-version
 cli_qdisc_fq_codel_la_LDFLAGS = -module -avoid-version
+cli_qdisc_hfsc_la_LDFLAGS = -module -avoid-version
 cli_cls_basic_la_LDFLAGS = -module -avoid-version
 cli_cls_cgroup_la_LDFLAGS = -module -avoid-version
 endif
diff --git a/lib/cli/qdisc/hfsc.c b/lib/cli/qdisc/hfsc.c
new file mode 100644
index 0000000..1e6878a
--- /dev/null
+++ b/lib/cli/qdisc/hfsc.c
@@ -0,0 +1,250 @@
+/*
+ * lib/cli/qdisc/hfsc.c     	HFSC module for CLI lib
+ *
+ *	This library is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation version 2.1
+ *	of the License.
+ *
+ * Copyright (c) 2014 Cong Wang <xiyou.wangcong@gmail.com>
+ */
+
+#include <netlink/cli/utils.h>
+#include <netlink/cli/tc.h>
+#include <netlink/route/qdisc/hfsc.h>
+
+static void print_qdisc_usage(void)
+{
+	printf(
+"Usage: nl-qdisc-add [...] hfsc [OPTIONS]...\n"
+"\n"
+"OPTIONS\n"
+"     --help                Show this help text.\n"
+"     --default=ID          Default class for unclassified traffic.\n"
+"\n"
+"EXAMPLE"
+"    # Create hfsc root qdisc 1: and direct unclassified traffic to class 1:10\n"
+"    nl-qdisc-add --dev=eth1 --parent=root --handle=1: hfsc --default=10\n");
+}
+
+static void hfsc_parse_qdisc_argv(struct rtnl_tc *tc, int argc, char **argv)
+{
+	struct rtnl_qdisc *qdisc = (struct rtnl_qdisc *) tc;
+
+	for (;;) {
+		int c, optidx = 0;
+		enum {
+			ARG_DEFAULT = 257,
+		};
+		static struct option long_opts[] = {
+			{ "help", 0, 0, 'h' },
+			{ "default", 1, 0, ARG_DEFAULT },
+			{ 0, 0, 0, 0 }
+		};
+
+		c = getopt_long(argc, argv, "hv", long_opts, &optidx);
+		if (c == -1)
+			break;
+
+		switch (c) {
+		case 'h':
+			print_qdisc_usage();
+			return;
+
+		case ARG_DEFAULT:
+			rtnl_qdisc_hfsc_set_defcls(qdisc, nl_cli_parse_u32(optarg));
+			break;
+		}
+	}
+}
+
+static void print_class_usage(void)
+{
+	printf(
+"Usage: nl-class-add [...] hfsc [OPTIONS]...\n"
+"\n"
+"OPTIONS\n"
+"     --help                Show this help text.\n"
+"     --ls=SC               Link-sharing service curve\n"
+"     --rt=SC               Real-time service curve\n"
+"     --sc=SC               Specifiy both of the above\n"
+"     --ul=SC               Upper limit\n"
+"     where SC := [ [ m1 bits ] d usec ] m2 bits\n"
+"\n"
+"EXAMPLE"
+"    # Attach class 1:1 to hfsc qdisc 1: and use rt and ls curve\n"
+"    nl-class-add --dev=eth1 --parent=1: --classid=1:1 hfsc --sc=m1:250,d:8,m2:100\n");
+}
+
+static int
+hfsc_get_sc(char *optarg, struct tc_service_curve *sc)
+{
+	unsigned int m1 = 0, d = 0, m2 = 0;
+	char *tmp = strdup(optarg);
+	char *p = tmp, *endptr;
+
+	if (!tmp)
+		return -ENOMEM;
+
+	p = strstr(p, "m1:");
+	if (p) {
+		char *q;
+		p += 3;
+		if (*p == 0)
+			goto err;
+		q = strchr(p, ',');
+		if (!q)
+			goto err;
+		*q = 0;
+		m1 = strtoul(p, &endptr, 10);
+		if (endptr == p)
+			goto err;
+		p = q + 1;
+        }
+
+	p = strstr(p, "d:");
+	if (p) {
+		char *q;
+		p += 2;
+		if (*p == 0)
+			goto err;
+		q = strchr(p, ',');
+		if (!q)
+			goto err;
+		*q = 0;
+		d = strtoul(p, &endptr, 10);
+		if (endptr == p)
+			goto err;
+		p = q + 1;
+        }
+
+	p = strstr(p, "m2:");
+	if (p) {
+		p += 3;
+		if (*p == 0)
+			goto err;
+		m2 = strtoul(p, &endptr, 10);
+		if (endptr == p)
+			goto err;
+        } else
+		goto err;
+
+	free(tmp);
+	sc->m1 = m1;
+	sc->d  = d;
+	sc->m2 = m2;
+	return 0;
+
+err:
+	free(tmp);
+	return -EINVAL;
+}
+
+static void hfsc_parse_class_argv(struct rtnl_tc *tc, int argc, char **argv)
+{
+	struct rtnl_class *class = (struct rtnl_class *) tc;
+	int arg_ok = 0, ret = -EINVAL;
+
+	for (;;) {
+		int c, optidx = 0;
+		enum {
+			ARG_RT = 257,
+			ARG_LS = 258,
+			ARG_SC,
+			ARG_UL,
+		};
+		static struct option long_opts[] = {
+			{ "help", 0, 0, 'h' },
+			{ "rt", 1, 0, ARG_RT },
+			{ "ls", 1, 0, ARG_LS },
+			{ "sc", 1, 0, ARG_SC },
+			{ "ul", 1, 0, ARG_UL },
+			{ 0, 0, 0, 0 }
+		};
+		struct tc_service_curve tsc;
+
+		c = getopt_long(argc, argv, "h", long_opts, &optidx);
+		if (c == -1)
+			break;
+
+		switch (c) {
+		case 'h':
+			print_class_usage();
+			return;
+
+		case ARG_RT:
+			ret = hfsc_get_sc(optarg, &tsc);
+			if (ret < 0) {
+				nl_cli_fatal(ret, "Unable to parse sc "
+					"\"%s\": Invalid format.", optarg);
+			}
+
+			rtnl_class_hfsc_set_rsc(class, &tsc);
+			arg_ok++;
+			break;
+
+		case ARG_LS:
+			ret = hfsc_get_sc(optarg, &tsc);
+			if (ret < 0) {
+				nl_cli_fatal(ret, "Unable to parse sc "
+					"\"%s\": Invalid format.", optarg);
+			}
+
+			rtnl_class_hfsc_set_fsc(class, &tsc);
+			arg_ok++;
+			break;
+
+		case ARG_SC:
+			ret = hfsc_get_sc(optarg, &tsc);
+			if (ret < 0) {
+				nl_cli_fatal(ret, "Unable to parse sc "
+					"\"%s\": Invalid format.", optarg);
+			}
+
+			rtnl_class_hfsc_set_rsc(class, &tsc);
+			rtnl_class_hfsc_set_fsc(class, &tsc);
+			arg_ok++;
+			break;
+
+		case ARG_UL:
+			ret = hfsc_get_sc(optarg, &tsc);
+			if (ret < 0) {
+				nl_cli_fatal(ret, "Unable to parse sc "
+					"\"%s\": Invalid format.", optarg);
+			}
+
+			rtnl_class_hfsc_set_usc(class, &tsc);
+			arg_ok++;
+			break;
+		}
+	}
+
+	if (!arg_ok)
+		nl_cli_fatal(ret, "Invalid arguments");
+}
+
+static struct nl_cli_tc_module hfsc_qdisc_module =
+{
+	.tm_name		= "hfsc",
+	.tm_type		= RTNL_TC_TYPE_QDISC,
+	.tm_parse_argv		= hfsc_parse_qdisc_argv,
+};
+
+static struct nl_cli_tc_module hfsc_class_module =
+{
+	.tm_name		= "hfsc",
+	.tm_type		= RTNL_TC_TYPE_CLASS,
+	.tm_parse_argv		= hfsc_parse_class_argv,
+};
+
+static void __init hfsc_init(void)
+{
+	nl_cli_tc_register(&hfsc_qdisc_module);
+	nl_cli_tc_register(&hfsc_class_module);
+}
+
+static void __exit hfsc_exit(void)
+{
+	nl_cli_tc_unregister(&hfsc_class_module);
+	nl_cli_tc_unregister(&hfsc_qdisc_module);
+}
diff --git a/lib/route/qdisc/hfsc.c b/lib/route/qdisc/hfsc.c
new file mode 100644
index 0000000..ddd1242
--- /dev/null
+++ b/lib/route/qdisc/hfsc.c
@@ -0,0 +1,351 @@
+/*
+ * lib/route/qdisc/hfsc.c	HFSC Qdisc
+ *
+ *	This library is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation version 2.1
+ *	of the License.
+ *
+ * Copyright (c) 2014 Cong Wang <xiyou.wangcong@gmail.com>
+ */
+
+/**
+ * @ingroup qdisc
+ * @ingroup class
+ * @defgroup qdisc_hfsc Hierarchical Fair Service Curve (HFSC)
+ * @{
+ */
+
+#include <netlink-private/netlink.h>
+#include <netlink-private/tc.h>
+#include <netlink/netlink.h>
+#include <netlink/cache.h>
+#include <netlink/utils.h>
+#include <netlink-private/route/tc-api.h>
+#include <netlink/route/qdisc.h>
+#include <netlink/route/class.h>
+#include <netlink/route/link.h>
+#include <netlink/route/qdisc/hfsc.h>
+
+/** @cond SKIP */
+#define SCH_HFSC_CLS_HAS_RSC		0x001
+#define SCH_HFSC_CLS_HAS_FSC		0x002
+#define SCH_HFSC_CLS_HAS_USC		0x004
+
+#define SCH_HFSC_QD_HAS_DEFCLS		0x01
+/** @endcond */
+
+static struct nla_policy hfsc_policy[TCA_HFSC_MAX + 1] = {
+	[TCA_HFSC_RSC]  = { .minlen = sizeof(struct tc_service_curve) },
+	[TCA_HFSC_FSC]  = { .minlen = sizeof(struct tc_service_curve) },
+	[TCA_HFSC_USC]  = { .minlen = sizeof(struct tc_service_curve) },
+};
+
+static int hfsc_qdisc_msg_parser(struct rtnl_tc *tc, void *data)
+{
+	struct rtnl_hfsc_qdisc *hfsc = data;
+	struct tc_hfsc_qopt *opts;
+
+	opts = (struct tc_hfsc_qopt *) tc->tc_opts->d_data;
+	hfsc->qh_defcls = opts->defcls;
+	hfsc->qh_mask |= SCH_HFSC_QD_HAS_DEFCLS;
+	return 0;
+}
+
+static int hfsc_class_msg_parser(struct rtnl_tc *tc, void *data)
+{
+	struct nlattr *tb[TCA_HFSC_MAX + 1];
+	struct rtnl_hfsc_class *hfsc = data;
+	int err;
+
+	if ((err = tca_parse(tb, TCA_HFSC_MAX, tc, hfsc_policy)) < 0)
+		return err;
+
+	if (tb[TCA_HFSC_RSC]) {
+		struct tc_service_curve tsc;
+
+		nla_memcpy(&tsc, tb[TCA_HFSC_RSC], sizeof(tsc));
+		hfsc->ch_rsc = tsc;
+		hfsc->ch_mask |= SCH_HFSC_CLS_HAS_RSC;
+	}
+
+	if (tb[TCA_HFSC_FSC]) {
+		struct tc_service_curve tsc;
+
+		nla_memcpy(&tsc, tb[TCA_HFSC_FSC], sizeof(tsc));
+		hfsc->ch_fsc = tsc;
+		hfsc->ch_mask |= SCH_HFSC_CLS_HAS_FSC;
+	}
+
+	if (tb[TCA_HFSC_USC]) {
+		struct tc_service_curve tsc;
+
+		nla_memcpy(&tsc, tb[TCA_HFSC_USC], sizeof(tsc));
+		hfsc->ch_usc = tsc;
+		hfsc->ch_mask |= SCH_HFSC_CLS_HAS_USC;
+	}
+
+	return 0;
+}
+
+static void hfsc_qdisc_dump_line(struct rtnl_tc *tc, void *data,
+				struct nl_dump_params *p)
+{
+	struct rtnl_hfsc_qdisc *hfsc = data;
+
+	if (!hfsc)
+		return;
+
+	if (hfsc->qh_mask & SCH_HFSC_QD_HAS_DEFCLS) {
+		char buf[64];
+		nl_dump(p, " default-class %s",
+			rtnl_tc_handle2str(hfsc->qh_defcls, buf, sizeof(buf)));
+	}
+}
+
+static void hfsc_dump_tsc(struct nl_dump_params *p, struct tc_service_curve *tsc)
+{
+	nl_dump(p, " m1 %u d %u m2 %u\n", tsc->m1, tsc->d, tsc->m2);
+}
+
+static void hfsc_class_dump_line(struct rtnl_tc *tc, void *data,
+				struct nl_dump_params *p)
+{
+	struct rtnl_hfsc_class *hfsc = data;
+
+	if (!hfsc)
+		return;
+	if (hfsc->ch_mask & SCH_HFSC_CLS_HAS_RSC)
+		hfsc_dump_tsc(p, &hfsc->ch_rsc);
+	if (hfsc->ch_mask & SCH_HFSC_CLS_HAS_FSC)
+		hfsc_dump_tsc(p, &hfsc->ch_fsc);
+	if (hfsc->ch_mask & SCH_HFSC_CLS_HAS_USC)
+		hfsc_dump_tsc(p, &hfsc->ch_usc);
+}
+
+static void hfsc_class_dump_details(struct rtnl_tc *tc, void *data,
+				   struct nl_dump_params *p)
+{
+	return;
+}
+
+static int hfsc_qdisc_msg_fill(struct rtnl_tc *tc, void *data,
+			      struct nl_msg *msg)
+{
+	struct rtnl_hfsc_qdisc *hfsc = data;
+	struct tc_hfsc_qopt opts = {0};
+
+	if (!hfsc)
+		BUG();
+
+	opts.defcls = hfsc->qh_defcls;
+	return nlmsg_append(msg, &opts, sizeof(opts), NL_DONTPAD);
+}
+
+static int hfsc_class_msg_fill(struct rtnl_tc *tc, void *data,
+			      struct nl_msg *msg)
+{
+	struct rtnl_hfsc_class *hfsc = data;
+	struct tc_service_curve tsc;
+
+	if (!hfsc)
+		BUG();
+
+	if (hfsc->ch_mask & SCH_HFSC_CLS_HAS_RSC) {
+		tsc = hfsc->ch_rsc;
+		NLA_PUT(msg, TCA_HFSC_RSC, sizeof(tsc), &tsc);
+	}
+
+	if (hfsc->ch_mask & SCH_HFSC_CLS_HAS_FSC) {
+		tsc = hfsc->ch_fsc;
+		NLA_PUT(msg, TCA_HFSC_FSC, sizeof(tsc), &tsc);
+	}
+
+	if (hfsc->ch_mask & SCH_HFSC_CLS_HAS_USC) {
+		tsc = hfsc->ch_usc;
+		NLA_PUT(msg, TCA_HFSC_USC, sizeof(tsc), &tsc);
+	}
+
+	return 0;
+
+nla_put_failure:
+	return -NLE_MSGSIZE;
+}
+
+static struct rtnl_tc_ops hfsc_qdisc_ops;
+static struct rtnl_tc_ops hfsc_class_ops;
+
+static struct rtnl_hfsc_qdisc *hfsc_qdisc_data(const struct rtnl_qdisc *qdisc, int *err)
+{
+	return rtnl_tc_data_check(TC_CAST(qdisc), &hfsc_qdisc_ops, err);
+}
+
+static struct rtnl_hfsc_class *hfsc_class_data(const struct rtnl_class *class, int *err)
+{
+	return rtnl_tc_data_check(TC_CAST(class), &hfsc_class_ops, err);
+}
+
+/**
+ * @name Attribute Modifications
+ * @{
+ */
+
+/**
+ * Return default class of HFSC qdisc
+ * @arg qdisc		hfsc qdisc object
+ *
+ * Returns the classid of the class where all unclassified traffic
+ * goes to.
+ *
+ * @return classid or TC_H_UNSPEC if unspecified.
+ */
+uint32_t rtnl_qdisc_hfsc_get_defcls(const struct rtnl_qdisc *qdisc)
+{
+	struct rtnl_hfsc_qdisc *hfsc;
+
+	if ((hfsc = hfsc_qdisc_data(qdisc, NULL)) &&
+	    (hfsc->qh_mask & SCH_HFSC_QD_HAS_DEFCLS))
+		return hfsc->qh_defcls;
+
+	return TC_H_UNSPEC;
+}
+
+/**
+ * Set default class of the hfsc qdisc to the specified value
+ * @arg qdisc		qdisc to change
+ * @arg defcls		new default class
+ */
+int rtnl_qdisc_hfsc_set_defcls(struct rtnl_qdisc *qdisc, uint32_t defcls)
+{
+	struct rtnl_hfsc_qdisc *hfsc;
+	int err;
+
+	if (!(hfsc = hfsc_qdisc_data(qdisc, &err)))
+		return err;
+
+	hfsc->qh_defcls = defcls;
+	hfsc->qh_mask |= SCH_HFSC_QD_HAS_DEFCLS;
+
+	return 0;
+}
+
+int rtnl_class_hfsc_get_rsc(const struct rtnl_class *class, struct tc_service_curve *tsc)
+{
+	struct rtnl_hfsc_class *hfsc;
+	int err = -NLE_OPNOTSUPP;
+
+	if ((hfsc = hfsc_class_data(class, &err)) &&
+	    (hfsc->ch_mask & SCH_HFSC_CLS_HAS_RSC)) {
+		*tsc = hfsc->ch_rsc;
+		return 0;
+	}
+
+	return err;
+}
+
+int rtnl_class_hfsc_set_rsc(struct rtnl_class *class, const struct tc_service_curve *tsc)
+{
+	struct rtnl_hfsc_class *hfsc;
+	int err;
+
+	if (!(hfsc = hfsc_class_data(class, &err)))
+		return err;
+
+	hfsc->ch_rsc = *tsc;
+	hfsc->ch_mask |= SCH_HFSC_CLS_HAS_RSC;
+
+	return 0;
+}
+
+int rtnl_class_hfsc_get_fsc(const struct rtnl_class *class, struct tc_service_curve *tsc)
+{
+	struct rtnl_hfsc_class *hfsc;
+	int err = -NLE_OPNOTSUPP;
+
+	if ((hfsc = hfsc_class_data(class, &err)) &&
+	    (hfsc->ch_mask & SCH_HFSC_CLS_HAS_FSC)) {
+		*tsc = hfsc->ch_fsc;
+		return 0;
+	}
+
+	return err;
+}
+
+int rtnl_class_hfsc_set_fsc(struct rtnl_class *class, const struct tc_service_curve *tsc)
+{
+	struct rtnl_hfsc_class *hfsc;
+	int err;
+
+	if (!(hfsc = hfsc_class_data(class, &err)))
+		return err;
+
+	hfsc->ch_fsc = *tsc;
+	hfsc->ch_mask |= SCH_HFSC_CLS_HAS_FSC;
+
+	return 0;
+}
+
+int rtnl_class_hfsc_get_usc(const struct rtnl_class *class, struct tc_service_curve *tsc)
+{
+	struct rtnl_hfsc_class *hfsc;
+	int err = -NLE_OPNOTSUPP;
+
+	if ((hfsc = hfsc_class_data(class, &err)) &&
+	    (hfsc->ch_mask & SCH_HFSC_CLS_HAS_USC)) {
+		*tsc = hfsc->ch_usc;
+		return 0;
+	}
+
+	return err;
+}
+
+int rtnl_class_hfsc_set_usc(struct rtnl_class *class, const struct tc_service_curve *tsc)
+{
+	struct rtnl_hfsc_class *hfsc;
+	int err;
+
+	if (!(hfsc = hfsc_class_data(class, &err)))
+		return err;
+
+	hfsc->ch_usc = *tsc;
+	hfsc->ch_mask |= SCH_HFSC_CLS_HAS_USC;
+
+	return 0;
+}
+
+/** @} */
+
+static struct rtnl_tc_ops hfsc_qdisc_ops = {
+	.to_kind		= "hfsc",
+	.to_type		= RTNL_TC_TYPE_QDISC,
+	.to_size		= sizeof(struct rtnl_hfsc_qdisc),
+	.to_msg_parser		= hfsc_qdisc_msg_parser,
+	.to_dump[NL_DUMP_LINE]	= hfsc_qdisc_dump_line,
+	.to_msg_fill		= hfsc_qdisc_msg_fill,
+};
+
+static struct rtnl_tc_ops hfsc_class_ops = {
+	.to_kind		= "hfsc",
+	.to_type		= RTNL_TC_TYPE_CLASS,
+	.to_size		= sizeof(struct rtnl_hfsc_class),
+	.to_msg_parser		= hfsc_class_msg_parser,
+	.to_dump = {
+	    [NL_DUMP_LINE]	= hfsc_class_dump_line,
+	    [NL_DUMP_DETAILS]	= hfsc_class_dump_details,
+	},
+	.to_msg_fill		= hfsc_class_msg_fill,
+};
+
+static void __init hfsc_init(void)
+{
+	rtnl_tc_register(&hfsc_qdisc_ops);
+	rtnl_tc_register(&hfsc_class_ops);
+}
+
+static void __exit hfsc_exit(void)
+{
+	rtnl_tc_unregister(&hfsc_qdisc_ops);
+	rtnl_tc_unregister(&hfsc_class_ops);
+}
+
+/** @} */
diff --git a/lib/route/qdisc/htb.c b/lib/route/qdisc/htb.c
index 5a61a4e..0277000 100644
--- a/lib/route/qdisc/htb.c
+++ b/lib/route/qdisc/htb.c
@@ -269,14 +269,14 @@ nla_put_failure:
 static struct rtnl_tc_ops htb_qdisc_ops;
 static struct rtnl_tc_ops htb_class_ops;
 
-static struct rtnl_htb_qdisc *htb_qdisc_data(struct rtnl_qdisc *qdisc)
+static struct rtnl_htb_qdisc *htb_qdisc_data(struct rtnl_qdisc *qdisc, int *err)
 {
-	return rtnl_tc_data_check(TC_CAST(qdisc), &htb_qdisc_ops);
+	return rtnl_tc_data_check(TC_CAST(qdisc), &htb_qdisc_ops, err);
 }
 
-static struct rtnl_htb_class *htb_class_data(struct rtnl_class *class)
+static struct rtnl_htb_class *htb_class_data(struct rtnl_class *class, int *err)
 {
-	return rtnl_tc_data_check(TC_CAST(class), &htb_class_ops);
+	return rtnl_tc_data_check(TC_CAST(class), &htb_class_ops, err);
 }
 
 /**
@@ -294,8 +294,8 @@ uint32_t rtnl_htb_get_rate2quantum(struct rtnl_qdisc *qdisc)
 {
 	struct rtnl_htb_qdisc *htb;
 
-	if ((htb = htb_qdisc_data(qdisc)) &&
-	    htb->qh_mask & SCH_HTB_HAS_RATE2QUANTUM)
+	if ((htb = htb_qdisc_data(qdisc, NULL)) &&
+	    (htb->qh_mask & SCH_HTB_HAS_RATE2QUANTUM))
 		return htb->qh_rate2quantum;
 
 	return 0;
@@ -304,9 +304,10 @@ uint32_t rtnl_htb_get_rate2quantum(struct rtnl_qdisc *qdisc)
 int rtnl_htb_set_rate2quantum(struct rtnl_qdisc *qdisc, uint32_t rate2quantum)
 {
 	struct rtnl_htb_qdisc *htb;
+	int err;
 
-	if (!(htb = htb_qdisc_data(qdisc)))
-		return -NLE_OPNOTSUPP;
+	if (!(htb = htb_qdisc_data(qdisc, &err)))
+		return err;
 
 	htb->qh_rate2quantum = rate2quantum;
 	htb->qh_mask |= SCH_HTB_HAS_RATE2QUANTUM;
@@ -327,7 +328,7 @@ uint32_t rtnl_htb_get_defcls(struct rtnl_qdisc *qdisc)
 {
 	struct rtnl_htb_qdisc *htb;
 
-	if ((htb = htb_qdisc_data(qdisc)) &&
+	if ((htb = htb_qdisc_data(qdisc, NULL)) &&
 	    htb->qh_mask & SCH_HTB_HAS_DEFCLS)
 		return htb->qh_defcls;
 
@@ -342,9 +343,10 @@ uint32_t rtnl_htb_get_defcls(struct rtnl_qdisc *qdisc)
 int rtnl_htb_set_defcls(struct rtnl_qdisc *qdisc, uint32_t defcls)
 {
 	struct rtnl_htb_qdisc *htb;
+	int err;
 
-	if (!(htb = htb_qdisc_data(qdisc)))
-		return -NLE_OPNOTSUPP;
+	if (!(htb = htb_qdisc_data(qdisc, &err)))
+		return err;
 
 	htb->qh_defcls = defcls;
 	htb->qh_mask |= SCH_HTB_HAS_DEFCLS;
@@ -356,7 +358,8 @@ uint32_t rtnl_htb_get_prio(struct rtnl_class *class)
 {
 	struct rtnl_htb_class *htb;
 
-	if ((htb = htb_class_data(class)) && htb->ch_mask & SCH_HTB_HAS_PRIO)
+	if ((htb = htb_class_data(class, NULL)) &&
+	    (htb->ch_mask & SCH_HTB_HAS_PRIO))
 		return htb->ch_prio;
 
 	return 0;
@@ -365,9 +368,10 @@ uint32_t rtnl_htb_get_prio(struct rtnl_class *class)
 int rtnl_htb_set_prio(struct rtnl_class *class, uint32_t prio)
 {
 	struct rtnl_htb_class *htb;
+	int err;
 
-	if (!(htb = htb_class_data(class)))
-		return -NLE_OPNOTSUPP;
+	if (!(htb = htb_class_data(class, &err)))
+		return err;
 
 	htb->ch_prio = prio;
 	htb->ch_mask |= SCH_HTB_HAS_PRIO;
@@ -385,7 +389,8 @@ uint32_t rtnl_htb_get_rate(struct rtnl_class *class)
 {
 	struct rtnl_htb_class *htb;
 
-	if ((htb = htb_class_data(class)) && htb->ch_mask & SCH_HTB_HAS_RATE)
+	if ((htb = htb_class_data(class, NULL)) &&
+	    (htb->ch_mask & SCH_HTB_HAS_RATE))
 		return htb->ch_rate.rs_rate;
 
 	return 0;
@@ -401,9 +406,10 @@ uint32_t rtnl_htb_get_rate(struct rtnl_class *class)
 int rtnl_htb_set_rate(struct rtnl_class *class, uint32_t rate)
 {
 	struct rtnl_htb_class *htb;
+	int err;
 
-	if (!(htb = htb_class_data(class)))
-		return -NLE_OPNOTSUPP;
+	if (!(htb = htb_class_data(class, &err)))
+		return err;
 
 	htb->ch_rate.rs_cell_log = UINT8_MAX; /* use default value */
 	htb->ch_rate.rs_rate = rate;
@@ -422,7 +428,8 @@ uint32_t rtnl_htb_get_ceil(struct rtnl_class *class)
 {
 	struct rtnl_htb_class *htb;
 
-	if ((htb = htb_class_data(class)) && htb->ch_mask & SCH_HTB_HAS_CEIL)
+	if ((htb = htb_class_data(class, NULL)) &&
+	    (htb->ch_mask & SCH_HTB_HAS_CEIL))
 		return htb->ch_ceil.rs_rate;
 
 	return 0;
@@ -438,9 +445,10 @@ uint32_t rtnl_htb_get_ceil(struct rtnl_class *class)
 int rtnl_htb_set_ceil(struct rtnl_class *class, uint32_t ceil)
 {
 	struct rtnl_htb_class *htb;
+	int err;
 
-	if (!(htb = htb_class_data(class)))
-		return -NLE_OPNOTSUPP;
+	if (!(htb = htb_class_data(class, &err)))
+		return err;
 
 	htb->ch_ceil.rs_cell_log = UINT8_MAX; /* use default value */
 	htb->ch_ceil.rs_rate = ceil;
@@ -459,7 +467,7 @@ uint32_t rtnl_htb_get_rbuffer(struct rtnl_class *class)
 {
 	struct rtnl_htb_class *htb;
 
-	if ((htb = htb_class_data(class)) &&
+	if ((htb = htb_class_data(class, NULL)) &&
 	     htb->ch_mask & SCH_HTB_HAS_RBUFFER)
 		return htb->ch_rbuffer;
 
@@ -474,9 +482,10 @@ uint32_t rtnl_htb_get_rbuffer(struct rtnl_class *class)
 int rtnl_htb_set_rbuffer(struct rtnl_class *class, uint32_t rbuffer)
 {
 	struct rtnl_htb_class *htb;
+	int err;
 
-	if (!(htb = htb_class_data(class)))
-		return -NLE_OPNOTSUPP;
+	if (!(htb = htb_class_data(class, &err)))
+		return err;
 
 	htb->ch_rbuffer = rbuffer;
 	htb->ch_mask |= SCH_HTB_HAS_RBUFFER;
@@ -494,7 +503,7 @@ uint32_t rtnl_htb_get_cbuffer(struct rtnl_class *class)
 {
 	struct rtnl_htb_class *htb;
 
-	if ((htb = htb_class_data(class)) &&
+	if ((htb = htb_class_data(class, NULL)) &&
 	     htb->ch_mask & SCH_HTB_HAS_CBUFFER)
 		return htb->ch_cbuffer;
 
@@ -509,9 +518,10 @@ uint32_t rtnl_htb_get_cbuffer(struct rtnl_class *class)
 int rtnl_htb_set_cbuffer(struct rtnl_class *class, uint32_t cbuffer)
 {
 	struct rtnl_htb_class *htb;
+	int err;
 
-	if (!(htb = htb_class_data(class)))
-		return -NLE_OPNOTSUPP;
+	if (!(htb = htb_class_data(class, &err)))
+		return err;
 
 	htb->ch_cbuffer = cbuffer;
 	htb->ch_mask |= SCH_HTB_HAS_CBUFFER;
@@ -531,7 +541,7 @@ uint32_t rtnl_htb_get_quantum(struct rtnl_class *class)
 {
 	struct rtnl_htb_class *htb;
 
-	if ((htb = htb_class_data(class)) &&
+	if ((htb = htb_class_data(class, NULL)) &&
 	    htb->ch_mask & SCH_HTB_HAS_QUANTUM)
 		return htb->ch_quantum;
 
@@ -550,9 +560,10 @@ uint32_t rtnl_htb_get_quantum(struct rtnl_class *class)
 int rtnl_htb_set_quantum(struct rtnl_class *class, uint32_t quantum)
 {
 	struct rtnl_htb_class *htb;
+	int err;
 
-	if (!(htb = htb_class_data(class)))
-		return -NLE_OPNOTSUPP;
+	if (!(htb = htb_class_data(class, &err)))
+		return err;
 
 	htb->ch_quantum = quantum;
 	htb->ch_mask |= SCH_HTB_HAS_QUANTUM;
@@ -568,16 +579,18 @@ int rtnl_htb_set_quantum(struct rtnl_class *class, uint32_t quantum)
  * 0, root classes have level (TC_HTB_MAXDEPTH - 1). Interior classes
  * have a level of one less than their parent.
  *
- * @return Level or -NLE_OPNOTSUPP
+ * @return Level or a negative error code.
  */
 int rtnl_htb_get_level(struct rtnl_class *class)
 {
 	struct rtnl_htb_class *htb;
+	int err = -NLE_OPNOTSUPP;
 
-	if ((htb = htb_class_data(class)) && htb->ch_mask & SCH_HTB_HAS_LEVEL)
+	if ((htb = htb_class_data(class, &err)) &&
+	    (htb->ch_mask & SCH_HTB_HAS_LEVEL))
 		return htb->ch_level;
 
-	return -NLE_OPNOTSUPP;
+	return err;
 }
 
 /**
@@ -595,9 +608,10 @@ int rtnl_htb_get_level(struct rtnl_class *class)
 int rtnl_htb_set_level(struct rtnl_class *class, int level)
 {
 	struct rtnl_htb_class *htb;
+	int err;
 
-	if (!(htb = htb_class_data(class)))
-		return -NLE_OPNOTSUPP;
+	if (!(htb = htb_class_data(class, &err)))
+		return err;
 
 	htb->ch_level = level;
 	htb->ch_mask |= SCH_HTB_HAS_LEVEL;
diff --git a/lib/route/tc.c b/lib/route/tc.c
index 0f150cc..34c3b57 100644
--- a/lib/route/tc.c
+++ b/lib/route/tc.c
@@ -1030,6 +1030,7 @@ void *rtnl_tc_data(struct rtnl_tc *tc)
  * Check traffic control object type and return private data section 
  * @arg tc		traffic control object
  * @arg ops		expected traffic control object operations
+ * @arg err		the place where saves the error code if fails
  *
  * Checks whether the traffic control object matches the type
  * specified with the traffic control object operations. If the
@@ -1040,8 +1041,10 @@ void *rtnl_tc_data(struct rtnl_tc *tc)
  *
  * @return Pointer to private tc data or NULL if type mismatches.
  */
-void *rtnl_tc_data_check(struct rtnl_tc *tc, struct rtnl_tc_ops *ops)
+void *rtnl_tc_data_check(struct rtnl_tc *tc, struct rtnl_tc_ops *ops, int *err)
 {
+	void *ret;
+
 	if (tc->tc_ops != ops) {
 		char buf[64];
 
@@ -1050,10 +1053,18 @@ void *rtnl_tc_data_check(struct rtnl_tc *tc, struct rtnl_tc_ops *ops)
 			 tc, ops->to_kind, tc->tc_ops->to_kind);
 		APPBUG(buf);
 
+		if (err)
+			*err = -NLE_OPNOTSUPP;
 		return NULL;
 	}
 
-	return rtnl_tc_data(tc);
+	ret = rtnl_tc_data(tc);
+	if (ret == NULL) {
+		if (err)
+			*err = -NLE_NOMEM;
+	}
+
+	return ret;
 }
 
 struct nl_af_group tc_groups[] = {