1/*
2
3	Tomato Firmware
4	Copyright (C) 2006-2007 Jonathan Zarate
5	$Id: qos.c 241182 2011-02-17 21:50:03Z $
6
7*/
8#include "rc.h"
9#include <sys/stat.h>
10#include <stdarg.h>
11#include <fcntl.h>
12#include <stdio.h>
13#include <stdlib.h>
14#include <unistd.h>
15#include <string.h>
16#include <signal.h>
17#include <syslog.h>
18#include <fcntl.h>
19#include <bcmnvram.h>
20#include <shutils.h>
21
22
23#define vstrsep(buf, sep, args...) _vstrsep(buf, sep, args, NULL)
24
25
26static const char *qosfn = "/tmp/qos/qos";
27
28static unsigned calc(unsigned bw, unsigned pct)
29{
30	unsigned n = ((unsigned long)bw * pct) / 100;
31	return (n < 2) ? 2 : n;
32}
33
34typedef struct sEVAL_CMD {
35	int siCount;
36	char *apCommand[64];
37}	EVAL_CMD;
38
39void printcmd(EVAL_CMD *ptIptCommand)
40{	int i;
41
42	printf("\n ### IPTABLE_CMD(%d): \"", ptIptCommand->siCount);
43	for (i = 0; i < ptIptCommand->siCount; i++)
44		if (ptIptCommand->apCommand[i])
45			printf("%s ", ptIptCommand->apCommand[i]);
46	printf("\"\n");
47}
48
49void del_iQosRules(void)
50{
51	/* Flush all rules in mangle table */
52	eval("iptables", "-t", "mangle", "-F");
53}
54
55int add_iQosRules(char *pcWANIF)
56{
57	char *buf;
58	char *g;
59	char *p;
60	char *addr_type, *addr;
61	char *proto;
62	char *port_type, *port;
63	char *class_prio;
64	char *ipp2p, *layer7;
65	char *bcount;
66	int class_num;
67	int proto_num;
68	int i;
69	char s[256];
70	int inuse;
71	int qosox_enable;
72	unsigned long min;
73	int bcount_enable;
74	int method;
75	int gum;
76	int sticky_enable;
77	char acClass[8];
78	char acByteCounter[32];
79	EVAL_CMD stIptCommand =
80	{ 3,
81		{
82		"iptables",
83		"-t",
84		"mangle",
85		NULL,
86		}
87	};
88
89	if (pcWANIF == NULL || !nvram_match("qos_enable", "1"))
90		return -1;
91		printf("\n%s: %s", __FUNCTION__, pcWANIF);
92
93	qosox_enable = bcount_enable = inuse = sticky_enable = 0;
94	method = atoi(nvram_safe_get("qos_method"));		  // strict rule ordering
95	gum = (method == 0) ? 0x100 : 0;
96	if (nvram_match("qos_sticky", "0"))
97		sticky_enable = 1;
98
99	eval("iptables", "-t", "mangle", "-N", "QOSO");
100	eval("iptables", "-t", "mangle", "-A", "QOSO", "-j",
101		"CONNMARK", "--restore-mark", "--mask", "0xff");
102	eval("iptables", "-t", "mangle", "-A", "QOSO", "-m",
103		"connmark", "!", "--mark", "0/0xff00", "-j", "RETURN");
104
105	g = buf = strdup(nvram_safe_get("qos_orules"));
106	while (g) {
107		/*	addr_type<addr<proto<port_type<port<<<<desc
108		addr_type:
109			0 = any
110			1 = dest ip
111			2 = src ip
112			3 = src mac
113		addr:
114			ip/mac if addr_type == 1-3
115		proto:
116			0-65535 = protocol
117			-1 = tcp or udp
118			-2 = any protocol
119		port_type:
120			if proto == -1,tcp,udp:
121				d = dest
122				s = src
123				x = both
124				a = any
125		port:
126			port # if proto == -1,tcp,udp
127		class_prio:
128			0-4, 0 being highest
129		*/
130		if ((p = strsep(&g, ">")) == NULL)
131			break;
132		i = vstrsep(p, "<", &addr_type, &addr, &proto, &port_type,
133			&port, &ipp2p, &layer7, &bcount, &class_prio, &p);
134		if (i == 9) {
135			class_prio = bcount;
136			bcount = NULL;
137		}
138		else if (i != 10)
139			continue;
140
141		class_num = atoi(class_prio);
142		if ((class_num < 0) || (class_num > 4))
143			continue;
144		i = 1 << class_num++;
145		if (method == 1) class_num |= 0x200;
146		if ((inuse & i) == 0) {
147			inuse |= i;
148			printf("inuse=%d\n", inuse);
149		}
150
151		/* Beginning of the Rule */
152		{
153			stIptCommand.siCount = 3;
154			stIptCommand.apCommand[stIptCommand.siCount++] = "-A";
155			if (qosox_enable)
156				stIptCommand.apCommand[stIptCommand.siCount++] = "QOSOX"; //Offset 4
157			else
158			stIptCommand.apCommand[stIptCommand.siCount++] = "QOSO";
159		}
160
161		/*
162		*	protocol & ports:
163		*/
164
165	{
166			proto_num = atoi(proto);
167		if (proto_num > -2) {
168			stIptCommand.apCommand[stIptCommand.siCount++] = "-p";
169		if (proto_num == 17)
170			stIptCommand.apCommand[stIptCommand.siCount++] = "udp"; //Offset 6
171		else if (proto_num != -1)
172			stIptCommand.apCommand[stIptCommand.siCount++] = proto;
173		else
174			stIptCommand.apCommand[stIptCommand.siCount++] = "tcp"; //Offset 6
175
176		if ((proto_num == 6) || (proto_num == 17) || (proto_num == -1)) {
177				if (*port_type != 'a') {
178				if ((*port_type == 'x') || (strchr(port, ','))) {
179		// dst-or-src port matches, and anything with multiple lists "," use mport
180			stIptCommand.apCommand[stIptCommand.siCount++] = "-m";
181			stIptCommand.apCommand[stIptCommand.siCount++] = "mport";
182				if (*port_type == 's')
183			stIptCommand.apCommand[stIptCommand.siCount++] = "--sports";
184				else
185			stIptCommand.apCommand[stIptCommand.siCount++] = "--dports";
186					}
187					else {
188				if (*port_type == 's')
189			stIptCommand.apCommand[stIptCommand.siCount++] = "--sport";
190				else if (*port_type == 'd')
191			stIptCommand.apCommand[stIptCommand.siCount++] = "--dport";
192				else
193			stIptCommand.apCommand[stIptCommand.siCount++] = "--port";
194					}
195				if (port && *port)
196			stIptCommand.apCommand[stIptCommand.siCount++] = port;
197					}
198				}
199			}
200		}
201
202		/*
203		*	MAC or IP address match:
204		*/
205	{
206		if ((*addr_type == '1') || (*addr_type == '2')) {	// match ip
207			if (strchr(addr, '-') != NULL) {
208				stIptCommand.apCommand[stIptCommand.siCount++] = "-m";
209				stIptCommand.apCommand[stIptCommand.siCount++] = "iprange";
210			if (*addr_type == '1')
211				stIptCommand.apCommand[stIptCommand.siCount++] = "--dst-range";
212			else
213				stIptCommand.apCommand[stIptCommand.siCount++] = "--src-range";
214			}
215			else {
216				if (*addr_type == '1')
217				stIptCommand.apCommand[stIptCommand.siCount++] = "-d";
218			else
219				stIptCommand.apCommand[stIptCommand.siCount++] = "-s";
220			}
221	}
222			else if (*addr_type == '3') {	// match mac
223				stIptCommand.apCommand[stIptCommand.siCount++] = "-m";
224				stIptCommand.apCommand[stIptCommand.siCount++] = "mac";
225				stIptCommand.apCommand[stIptCommand.siCount++] = "--mac-source";
226			}
227			if (*addr_type != '0')
228				stIptCommand.apCommand[stIptCommand.siCount++] = addr;
229		}
230
231		/* End of the rule */
232		{
233			class_num |= gum;
234			if (sticky_enable)
235				class_num &= 0xFF;
236			sprintf(acClass, "0x%x/0xFF", class_num);
237			stIptCommand.apCommand[stIptCommand.siCount++] = "-j";
238			stIptCommand.apCommand[stIptCommand.siCount++] = "CONNMARK";
239			stIptCommand.apCommand[stIptCommand.siCount++] = "--set-mark";
240			stIptCommand.apCommand[stIptCommand.siCount++] = acClass;
241			stIptCommand.apCommand[stIptCommand.siCount++] = NULL;
242		}
243		printcmd(&stIptCommand);
244		_eval(stIptCommand.apCommand, NULL, 4, NULL);
245		if (proto_num == -1) {
246			stIptCommand.apCommand[6] = "udp";
247			printcmd(&stIptCommand);
248			_eval(stIptCommand.apCommand, NULL, 4, NULL);
249		}
250			stIptCommand.siCount = 3;
251			stIptCommand.apCommand[stIptCommand.siCount++] = "-A";
252		if(qosox_enable)
253			stIptCommand.apCommand[stIptCommand.siCount++] = "QOSOX";
254		else
255			stIptCommand.apCommand[stIptCommand.siCount++] = "QOSO";
256			stIptCommand.apCommand[stIptCommand.siCount++] = "-j";
257			stIptCommand.apCommand[stIptCommand.siCount++] = "RETURN";
258			stIptCommand.apCommand[stIptCommand.siCount++] = NULL;
259			printcmd(&stIptCommand);
260		_eval(stIptCommand.apCommand, NULL, 4, NULL);
261	}
262	free(buf);
263
264
265	/*
266	*	The default class:
267	*/
268
269	{	char acClass[4];
270
271		i = atoi(nvram_safe_get("qos_default"));
272		if ((i < 0) || (i > 9)) i = 3;	// "low"
273		class_num = i + 1;
274		if (method == 1) class_num |= 0x200;
275		sprintf(acClass, "0x%x", class_num);
276		eval("iptables", "-t", "mangle", "-A", qosox_enable ? "QOSOX" : "QOSO",
277		"-j", "CONNMARK", "--set-mark", acClass);
278		eval("iptables", "-t", "mangle", "-A", qosox_enable ? "QOSOX" : "QOSO",
279                "-j", "RETURN");
280	}
281
282		eval("iptables", "-t", "mangle", "-A", "FORWARD", "-o", pcWANIF, "-j", qosox_enable ? "QOSOX" : "QOSO");
283
284	/*
285	*	Ingress rules:
286	*/
287	{	i = atoi(nvram_safe_get("qos_default"));
288		inuse |= (1 << i) | 1;	// default and highest are always built
289		sprintf(s, "%d", inuse);
290		nvram_set("qos_inuse", s);	  // create the inuse NVRAM here
291
292		g = buf = strdup(nvram_safe_get("qos_irates"));
293		for (i = 0; i < 5; i++) {
294			if ((!g) || ((p = strsep(&g, ",")) == NULL)) continue;
295			if ((inuse & (1 << i)) == 0) continue;
296			if (atoi(p) > 0) {// if ibound rules are set, use the PRE-ROUTE
297				eval("iptables", "-t", "mangle", "-A", "PREROUTING",
298				"-i", pcWANIF, "-j", "CONNMARK",
299				"--restore-mark", "--mask", "0xff");
300				break;
301			}
302		}
303		free(buf);
304	}
305
306}
307
308/* Tc */
309int start_iQos(void)
310{
311	int i;
312	char *buf, *g, *p;
313	unsigned int rate;
314	unsigned int ceil;
315	unsigned int bw;
316	unsigned int mtu;
317	FILE *f;
318	int x;
319	int inuse;
320	char s[256];
321	int first;
322	char burst_root[32];
323	char burst_leaf[32];
324
325	if (!nvram_match("qos_enable", "1")) return;
326	if ((f = fopen(qosfn, "w")) == NULL) return;
327
328	i = atoi(nvram_safe_get("qos_burst0"));
329	if (i > 0) sprintf(burst_root, "burst %dk", i);
330		else burst_root[0] = 0;
331	i = atoi(nvram_safe_get("qos_burst1"));
332
333	if (i > 0) sprintf(burst_leaf, "burst %dk", i);
334		else burst_leaf[0] = 0;
335	/* Egress OBW  -- set the HTB shaper (Classful Qdisc)
336	* the BW is set here for each class
337	*/
338
339	mtu = strtoul(nvram_safe_get("wan_mtu"), NULL, 10);
340	bw = strtoul(nvram_safe_get("qos_obw"), NULL, 10);
341
342	fprintf(f,
343		"#!/bin/sh\n"
344		"I=%s\n"
345		"SFQ=\"sfq perturb 10\"\n"
346		"TQA=\"tc qdisc add dev $I\"\n"
347		"TCA=\"tc class add dev $I\"\n"
348		"TFA=\"tc filter add dev $I\"\n"
349		"\n"
350		"case \"$1\" in\n"
351		"start)\n"
352		"\ttc qdisc del dev $I root 2>/dev/null\n"
353		"\t$TQA root handle 1: htb default %u\n"
354		"\t$TCA parent 1: classid 1:1 htb rate %ukbit ceil %ukbit %s\n",
355		nvram_safe_get("wan_ifname"),
356		(atoi(nvram_safe_get("qos_default")) + 1) * 10,	bw, bw, burst_root);
357		inuse = atoi(nvram_safe_get("qos_inuse"));
358
359	g = buf = strdup(nvram_safe_get("qos_orates"));
360	for (i = 0; i < 10; ++i) {
361	if ((!g) || ((p = strsep(&g, ",")) == NULL)) break;
362		if ((inuse & (1 << i)) == 0) continue;
363		if ((sscanf(p, "%u-%u", &rate, &ceil) != 2) || (rate < 1)) continue;
364		if (ceil > 0) sprintf(s, "ceil %ukbit ", calc(bw, ceil));
365			else s[0] = 0;
366		x = (i + 1) * 10;
367		fprintf(f,
368			"# egress %d: %u-%u%%\n"
369			"\t$TCA parent 1:1 classid 1:%d htb rate %ukbit %s %s prio %d quantum %u\n"
370			"\t$TQA parent 1:%d handle %d: $SFQ\n"
371			"\t$TFA parent 1: prio %d protocol ip handle %d fw flowid 1:%d\n",
372				i, rate, ceil,
373				x, calc(bw, rate), s, burst_leaf, (i >= 6) ? 7 : (i + 1), mtu,
374				x, x,
375				x, i + 1, x);
376	}
377	free(buf);
378
379
380	if (nvram_match("qos_ack", "1")) {
381
382		fprintf(f,
383			"\n"
384			"\t$TFA parent 1: prio 15 protocol ip u32 "
385			"match ip protocol 6 0xff "		// TCP
386			"match u8 0x05 0x0f at 0 "		// IP header length
387			"match u16 0x0000 0xffc0 at 2 "	// total length(0-63)
388			"match u8 0x10 0xff at 33 "		// ACK only
389			"flowid 1:10\n");
390
391	}
392	if (nvram_match("qos_icmp", "1")) {
393
394		fputs("\n\t$TFA parent 1: prio 14 protocol ip u32 match"
395			"ip protocol 1 0xff flowid 1:10\n", f);
396		fputs("\n\t$TFA parent 1: prio 14 protocol ip u32 match"
397			"ip protocol 1 0xff flowid 1:10\n", stderr);
398	}
399
400	// ingress
401	first = 1;
402	bw = strtoul(nvram_safe_get("qos_ibw"), NULL, 10);
403	if (bw > 0) {
404	g = buf = strdup(nvram_safe_get("qos_irates"));
405	for (i = 0; i < 10; ++i) {
406		if ((!g) || ((p = strsep(&g, ",")) == NULL)) break;
407		if ((inuse & (1 << i)) == 0) continue;
408		if ((rate = atoi(p)) < 1) continue;	// 0 = off
409
410		if (first) {
411			first = 0;
412			fprintf(f,
413				"\n"
414				"\ttc qdisc del dev $I ingress 2>/dev/null\n"
415				"\t$TQA handle ffff: ingress\n");
416		}
417
418		// rate in kb/s
419		unsigned int u = calc(bw, rate);
420
421		// burst rate
422		unsigned int v = u / 25;
423		if (v < 50) v = 50;
424
425		x = i + 1;
426		fprintf(f,
427			"# ingress %d: %u%%\n"
428			"\t$TFA parent ffff: prio %d protocol ip handle %d"
429				" fw police rate %ukbit burst %ukbit drop flowid ffff:%d\n",
430				i, rate, x, x, u, v, x);
431	}
432
433}
434
435	free(buf);
436
437	fputs(
438		"\t;;\n"
439		"stop)\n"
440		"\ttc qdisc del dev $I root 2>/dev/null\n"
441		"\ttc qdisc del dev $I ingress 2>/dev/null\n"
442		"\t;;\n"
443		"*)\n"
444		"\ttc -s -d qdisc ls dev $I\n"
445		"\techo\n"
446		"\ttc -s -d class ls dev $I\n"
447		"esac\n",
448		f);
449
450	fclose(f);
451	chmod(qosfn, 0700);
452	eval((char *)qosfn, "start");
453
454}
455
456
457void stop_iQos(void)
458{
459		eval((char *)qosfn, "stop");
460}
461
462/* va_list is part of stdarg.h */
463int _vstrsep(char *buf, const char *sep, ...)
464{
465	va_list ap;
466	char **p;
467	int n;
468	n = 0;
469	va_start(ap, sep);
470	while ((p = va_arg(ap, char **)) != NULL) {
471		if ((*p = strsep(&buf, sep)) == NULL) break;
472		++n;
473	}
474	va_end(ap);
475	return n;
476}
477