shaper: implemented leaf qdisc attachment

6 files changed, 238 insertions, 20 deletions

diff --git a/accel-pppd/shaper/CMakeLists.txt b/accel-pppd/shaper/CMakeLists.txt
index abfd8ecb..515fd839 100644
--- a/accel-pppd/shaper/CMakeLists.txt
+++ b/accel-pppd/shaper/CMakeLists.txt
@@ -1,4 +1,4 @@
-ADD_LIBRARY(shaper SHARED shaper.c limiter.c tc_core.c libnetlink.c)
+ADD_LIBRARY(shaper SHARED shaper.c limiter.c leaf_qdisc.c tc_core.c libnetlink.c)
 
 INSTALL(TARGETS shaper
 	LIBRARY DESTINATION lib/accel-ppp
diff --git a/accel-pppd/shaper/leaf_qdisc.c b/accel-pppd/shaper/leaf_qdisc.c
new file mode 100644
index 00000000..51b8c8d7
--- /dev/null
+++ b/accel-pppd/shaper/leaf_qdisc.c
@@ -0,0 +1,186 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <errno.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <sys/ioctl.h>
+#include <linux/if.h>
+#include <linux/if_ether.h>
+#include <linux/pkt_cls.h>
+#include <linux/pkt_sched.h>
+#include <linux/tc_act/tc_mirred.h>
+#include <linux/tc_act/tc_skbedit.h>
+
+#include "log.h"
+#include "ppp.h"
+
+#include "shaper.h"
+#include "tc_core.h"
+#include "libnetlink.h"
+
+static int parse_size(const char *str, int *r)
+{
+	double sz;
+	char *endptr;
+
+	sz = strtod(str, &endptr);
+
+	if (endptr == str)
+		return -1;
+	
+	if (*endptr == 0) {
+		*r = sz;
+		return 0;
+	}
+	
+	if (strcmp(endptr, "kb") == 0 || strcmp(endptr, "k") == 0)
+		*r = sz * 1024;
+	else if (strcmp(endptr, "mb") == 0 || strcmp(endptr, "m") == 0)
+		*r = sz * 1024 * 1024;
+	else if (strcmp(endptr, "gb") == 0 || strcmp(endptr, "g") == 0)
+		*r = sz * 1024 * 1024 * 1024;
+	else if (strcmp(endptr, "kbit") == 0)
+		*r = sz * 1024  / 8;
+	else if (strcmp(endptr, "mbit") == 0)
+		*r = sz * 1024 * 1024 / 8;
+	else if (strcmp(endptr, "gbit") == 0)
+		*r = sz * 1024 * 1024 * 1024 / 8;
+	else if (strcmp(endptr, "b") == 0)
+		*r = sz;
+	else
+		return -1;
+
+	return 0;
+}
+
+static int parse_int(const char *str, int *r)
+{
+	char *endptr;
+
+	*r = strtol(str, &endptr, 10);
+
+	return *endptr != 0;
+}
+
+static int parse_u32(const char *str, int *r)
+{
+	char *endptr;
+
+	*r = strtol(str, &endptr, 10);
+
+	return *endptr != 0 || *r < 0;
+}
+
+static int parse_sfq(char *str)
+{
+	char *ptr1, *ptr2;
+
+	if (!*str)
+		goto out;
+	
+	while (1) {
+		for (ptr1 = str + 1; *ptr1 && *ptr1 != ' '; ptr1++);
+
+		if (!*ptr1)
+			return -1;
+	
+		*ptr1 = 0;
+
+		for (ptr1++; *ptr1 && *ptr1 == ' '; ptr1++);
+		
+		if (!*ptr1)
+			return -1;
+
+		for (ptr2 = ptr1 + 1; *ptr2 && *ptr2 != ' '; ptr2++);
+
+		if (*ptr2) {
+			*ptr2 = 0;
+			for (ptr2++; *ptr2 && *ptr2 == ' '; ptr2++);
+		}
+
+		if (strcmp(str, "quantum") == 0) {
+			if (parse_size(ptr1, &conf_lq_arg1))
+				return -1;
+		} else if (strcmp(str, "perturb") == 0) {
+			if (parse_int(ptr1, &conf_lq_arg2))
+				return -1;
+		} else if (strcmp(str, "limit") == 0) {
+			if (parse_u32(ptr1, &conf_lq_arg3))
+				return -1;
+		} else
+			return -1;
+
+		if (*ptr2 == 0)
+			break;
+
+		str = ptr2;
+	}
+
+out:
+	conf_leaf_qdisc = LEAF_QDISC_SFQ;
+
+	return 0;
+}
+
+void leaf_qdisc_parse(const char *opt)
+{
+	char *ptr1;
+	char *str = strdup(opt);
+
+	for (ptr1 = str; *ptr1 && *ptr1 != ' '; ptr1++);
+
+	if (*ptr1) {
+		*ptr1 = 0;
+		for (ptr1++; *ptr1 && *ptr1 == ' '; ptr1++);
+	}
+
+	if (strcmp(str, "sfq") == 0) {
+		if (parse_sfq(ptr1))
+			goto out_err;
+	} else
+		log_emerg("shaper: unknown leaf-qdisc '%s'\n", str);
+	
+	free(str);
+	
+	return;
+out_err:
+	log_emerg("shaper: failed to parse '%s'\n", opt);
+}
+
+static int qdisc_sfq(struct qdisc_opt *qopt, struct nlmsghdr *n)
+{
+	struct tc_sfq_qopt opt = {
+		.quantum = conf_lq_arg1,
+		.perturb_period = conf_lq_arg2,
+		.limit = conf_lq_arg3,
+	};
+	
+	addattr_l(n, 1024, TCA_OPTIONS, &opt, sizeof(opt));
+
+	return 0;
+}
+
+static int install_sfq(struct rtnl_handle *rth, int ifindex, int parent, int handle)
+{
+	struct qdisc_opt opt = {
+		.kind = "sfq",
+		.handle = handle,
+		.parent = parent,
+		.qdisc = qdisc_sfq,
+	};
+
+	return tc_qdisc_modify(rth, ifindex, RTM_NEWQDISC, NLM_F_EXCL|NLM_F_CREATE, &opt);
+}
+
+
+int install_leaf_qdisc(struct rtnl_handle *rth, int ifindex, int parent, int handle)
+{
+	if (conf_leaf_qdisc == LEAF_QDISC_SFQ)
+		return install_sfq(rth, ifindex, parent, handle);
+	
+	return 0;
+}
+
diff --git a/accel-pppd/shaper/libnetlink.h b/accel-pppd/shaper/libnetlink.h
index 63ba5ad1..f68bf8a1 100644
--- a/accel-pppd/shaper/libnetlink.h
+++ b/accel-pppd/shaper/libnetlink.h
@@ -8,6 +8,9 @@
 #include <linux/if_addr.h>
 #include <linux/neighbour.h>
 
+#define TCA_BUF_MAX 64*1024
+#define MAX_MSG 16384
+
 struct rtnl_handle
 {
 	int			fd;
diff --git a/accel-pppd/shaper/limiter.c b/accel-pppd/shaper/limiter.c
index f368b331..92e343b7 100644
--- a/accel-pppd/shaper/limiter.c
+++ b/accel-pppd/shaper/limiter.c
@@ -21,22 +21,6 @@
 #include "tc_core.h"
 #include "libnetlink.h"
 
-#define TCA_BUF_MAX 64*1024
-#define MAX_MSG 16384
-
-struct qdisc_opt
-{
-	char *kind;
-	int handle;
-	int parent;
-	double latency;
-	int rate;
-	int buffer;
-	int quantum;
-	int defcls;
-	int (*qdisc)(struct qdisc_opt *opt, struct nlmsghdr *n);
-};
-
 static int qdisc_tbf(struct qdisc_opt *qopt, struct nlmsghdr *n)
 {
 	struct tc_tbf_qopt opt;
@@ -121,7 +105,7 @@ static int qdisc_htb_class(struct qdisc_opt *qopt, struct nlmsghdr *n)
 	return 0;
 }
 
-static int tc_qdisc_modify(struct rtnl_handle *rth, int ifindex, int cmd, unsigned flags, struct qdisc_opt *opt)
+int tc_qdisc_modify(struct rtnl_handle *rth, int ifindex, int cmd, unsigned flags, struct qdisc_opt *opt)
 {
 	struct {
 			struct nlmsghdr 	n;
@@ -446,13 +430,19 @@ int install_limiter(struct ppp_t *ppp, int down_speed, int down_burst, int up_sp
 
 	if (conf_down_limiter == LIM_TBF)
 		r = install_tbf(&rth, ppp->ifindex, down_speed, down_burst);
-	else
+	else {
 		r = install_htb(&rth, ppp->ifindex, down_speed, down_burst);
+		if (r == 0)
+			r = install_leaf_qdisc(&rth, ppp->ifindex, 0x00010001, 0x00020000);
+	}
 
 	if (conf_up_limiter == LIM_POLICE)
 		r = install_police(&rth, ppp->ifindex, up_speed, up_burst);
-	else
+	else {
 		r = install_htb_ifb(&rth, ppp->ifindex, ppp->unit_idx + 1, up_speed, up_burst);
+		if (r == 0)
+			r = install_leaf_qdisc(&rth, conf_ifb_ifindex, 0x00010001 + ppp->unit_idx + 1, (1 + ppp->unit_idx + 1) << 16);
+	}
 	
 	rtnl_close(&rth);
 
diff --git a/accel-pppd/shaper/shaper.c b/accel-pppd/shaper/shaper.c
index 0ea2e373..8ae2630a 100644
--- a/accel-pppd/shaper/shaper.c
+++ b/accel-pppd/shaper/shaper.c
@@ -44,6 +44,11 @@ int conf_ifb_ifindex;
 int conf_up_limiter = LIM_POLICE;
 int conf_down_limiter = LIM_TBF;
 
+int conf_leaf_qdisc;
+int conf_lq_arg1;
+int conf_lq_arg2;
+int conf_lq_arg3;
+
 static int temp_down_speed;
 static int temp_up_speed;
 
@@ -895,6 +900,13 @@ static void load_config(void)
 		conf_up_limiter = LIM_POLICE;
 	}
 
+	opt = conf_get_opt("shaper", "leaf-qdisc");
+	if (opt)
+		leaf_qdisc_parse(opt);
+	else
+		conf_leaf_qdisc = 0;
+
+
 	opt = conf_get_opt("shaper", "verbose");
 	if (opt && atoi(opt) > 0)
 		conf_verbose = 1;
diff --git a/accel-pppd/shaper/shaper.h b/accel-pppd/shaper/shaper.h
index bd2a718c..41e6e437 100644
--- a/accel-pppd/shaper/shaper.h
+++ b/accel-pppd/shaper/shaper.h
@@ -5,6 +5,24 @@
 #define LIM_TBF 1
 #define LIM_HTB 2
 
+#define LEAF_QDISC_SFQ 1
+
+struct rtnl_handle;
+struct nlmsghdr;
+
+struct qdisc_opt
+{
+	char *kind;
+	int handle;
+	int parent;
+	double latency;
+	int rate;
+	int buffer;
+	int quantum;
+	int defcls;
+	int (*qdisc)(struct qdisc_opt *opt, struct nlmsghdr *n);
+};	
+
 extern int conf_up_limiter;
 extern int conf_down_limiter;
 
@@ -16,9 +34,18 @@ extern int conf_quantum;
 extern int conf_r2q;
 extern int conf_cburst;
 extern int conf_ifb_ifindex;
+extern int conf_leaf_qdisc;
+extern int conf_lq_arg1;
+extern int conf_lq_arg2;
+extern int conf_lq_arg3;
 
 int install_limiter(struct ppp_t *ppp, int down_speed, int down_burst, int up_speed, int up_burst);
 int remove_limiter(struct ppp_t *ppp);
+int install_leaf_qdisc(struct rtnl_handle *rth, int ifindex, int parent, int handle);
 int init_ifb(const char *);
 
+void leaf_qdisc_parse(const char *);
+
+int tc_qdisc_modify(struct rtnl_handle *rth, int ifindex, int cmd, unsigned flags, struct qdisc_opt *opt);
+
 #endif