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