tc/f_u32.c

   1 /*
   2  * q_u32.c              U32 filter.
   3  *
   4  *              This program is free software; you can u32istribute it and/or
   5  *              modify it under the terms of the GNU General Public License
   6  *              as published by the Free Software Foundation; either version
   7  *              2 of the License, or (at your option) any later version.
   8  *
   9  * Authors:     Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
  10  *              Match mark added by Catalin(ux aka Dino) BOIE <catab at umbrella.ro> [5 nov 2004]
  11  *
  12  */
  13
  14 #include <stdio.h>
  15 #include <stdlib.h>
  16 #include <unistd.h>
  17 #include <syslog.h>
  18 #include <fcntl.h>
  19 #include <sys/socket.h>
  20 #include <netinet/in.h>
  21 #include <arpa/inet.h>
  22 #include <string.h>
  23 #include <linux/if.h>
  24 #include <linux/if_ether.h>
  25
  26 #include "utils.h"
  27 #include "tc_util.h"
  28
  29 extern int show_pretty;
  30
  31 static void explain(void)
  32 {
  33         fprintf(stderr, "Usage: ... u32 [ match SELECTOR ... ] [ link HTID ]"
  34                 " [ classid CLASSID ]\n");
  35         fprintf(stderr, "               [ police POLICE_SPEC ]"
  36                 " [ offset OFFSET_SPEC ]\n");
  37         fprintf(stderr, "               [ ht HTID ] [ hashkey HASHKEY_SPEC ]\n");
  38         fprintf(stderr, "               [ sample SAMPLE ]\n");
  39         fprintf(stderr, "or         u32 divisor DIVISOR\n");
  40         fprintf(stderr, "\n");
  41         fprintf(stderr, "Where: SELECTOR := SAMPLE SAMPLE ...\n");
  42         fprintf(stderr, "       SAMPLE := { ip | ip6 | udp | tcp | icmp |"
  43                 " u{32|16|8} | mark } SAMPLE_ARGS [divisor DIVISOR]\n");
  44         fprintf(stderr, "       FILTERID := X:Y:Z\n");
  45         fprintf(stderr, "\nNOTE: CLASSID is parsed at hexadecimal input.\n");
  46 }
  47
  48 #define usage() return(-1)
  49
  50 int get_u32_handle(__u32 *handle, char *str)
  51 {
  52         __u32 htid=0, hash=0, nodeid=0;
  53         char *tmp = strchr(str, ':');
  54
  55         if (tmp == NULL) {
  56                 if (memcmp("0x", str, 2) == 0)
  57                         return get_u32(handle, str, 16);
  58                 return -1;
  59         }
  60         htid = strtoul(str, &tmp, 16);
  61         if (tmp == str && *str != ':' && *str != 0)
  62                 return -1;
  63         if (htid>=0x1000)
  64                 return -1;
  65         if (*tmp) {
  66                 str = tmp+1;
  67                 hash = strtoul(str, &tmp, 16);
  68                 if (tmp == str && *str != ':' && *str != 0)
  69                         return -1;
  70                 if (hash>=0x100)
  71                         return -1;
  72                 if (*tmp) {
  73                         str = tmp+1;
  74                         nodeid = strtoul(str, &tmp, 16);
  75                         if (tmp == str && *str != 0)
  76                                 return -1;
  77                         if (nodeid>=0x1000)
  78                                 return -1;
  79                 }
  80         }
  81         *handle = (htid<<20)|(hash<<12)|nodeid;
  82         return 0;
  83 }
  84
  85 char * sprint_u32_handle(__u32 handle, char *buf)
  86 {
  87         int bsize = SPRINT_BSIZE-1;
  88         __u32 htid = TC_U32_HTID(handle);
  89         __u32 hash = TC_U32_HASH(handle);
  90         __u32 nodeid = TC_U32_NODE(handle);
  91         char *b = buf;
  92
  93         if (handle == 0) {
  94                 snprintf(b, bsize, "none");
  95                 return b;
  96         }
  97         if (htid) {
  98                 int l = snprintf(b, bsize, "%x:", htid>>20);
  99                 bsize -= l;
 100                 b += l;
 101         }
 102         if (nodeid|hash) {
 103                 if (hash) {
 104                         int l = snprintf(b, bsize, "%x", hash);
 105                         bsize -= l;
 106                         b += l;
 107                 }
 108                 if (nodeid) {
 109                         int l = snprintf(b, bsize, ":%x", nodeid);
 110                         bsize -= l;
 111                         b += l;
 112                 }
 113         }
 114         if (show_raw)
 115                 snprintf(b, bsize, "[%08x] ", handle);
 116         return buf;
 117 }
 118
 119 static int pack_key(struct tc_u32_sel *sel, __u32 key, __u32 mask,
 120                     int off, int offmask)
 121 {
 122         int i;
 123         int hwm = sel->nkeys;
 124
 125         key &= mask;
 126
 127         for (i=0; i<hwm; i++) {
 128                 if (sel->keys[i].off == off && sel->keys[i].offmask == offmask) {
 129                         __u32 intersect = mask&sel->keys[i].mask;
 130
 131                         if ((key^sel->keys[i].val) & intersect)
 132                                 return -1;
 133                         sel->keys[i].val |= key;
 134                         sel->keys[i].mask |= mask;
 135                         return 0;
 136                 }
 137         }
 138
 139         if (hwm >= 128)
 140                 return -1;
 141         if (off % 4)
 142                 return -1;
 143         sel->keys[hwm].val = key;
 144         sel->keys[hwm].mask = mask;
 145         sel->keys[hwm].off = off;
 146         sel->keys[hwm].offmask = offmask;
 147         sel->nkeys++;
 148         return 0;
 149 }
 150
 151 static int pack_key32(struct tc_u32_sel *sel, __u32 key, __u32 mask,
 152                       int off, int offmask)
 153 {
 154         key = htonl(key);
 155         mask = htonl(mask);
 156         return pack_key(sel, key, mask, off, offmask);
 157 }
 158
 159 static int pack_key16(struct tc_u32_sel *sel, __u32 key, __u32 mask,
 160                       int off, int offmask)
 161 {
 162         if (key > 0xFFFF || mask > 0xFFFF)
 163                 return -1;
 164
 165         if ((off & 3) == 0) {
 166                 key <<= 16;
 167                 mask <<= 16;
 168         }
 169         off &= ~3;
 170         key = htonl(key);
 171         mask = htonl(mask);
 172
 173         return pack_key(sel, key, mask, off, offmask);
 174 }
 175
 176 static int pack_key8(struct tc_u32_sel *sel, __u32 key, __u32 mask, int off, int offmask)
 177 {
 178         if (key > 0xFF || mask > 0xFF)
 179                 return -1;
 180
 181         if ((off & 3) == 0) {
 182                 key <<= 24;
 183                 mask <<= 24;
 184         } else if ((off & 3) == 1) {
 185                 key <<= 16;
 186                 mask <<= 16;
 187         } else if ((off & 3) == 2) {
 188                 key <<= 8;
 189                 mask <<= 8;
 190         }
 191         off &= ~3;
 192         key = htonl(key);
 193         mask = htonl(mask);
 194
 195         return pack_key(sel, key, mask, off, offmask);
 196 }
 197
 198
 199 int parse_at(int *argc_p, char ***argv_p, int *off, int *offmask)
 200 {
 201         int argc = *argc_p;
 202         char **argv = *argv_p;
 203         char *p = *argv;
 204
 205         if (argc <= 0)
 206                 return -1;
 207
 208         if (strlen(p) > strlen("nexthdr+") &&
 209             memcmp(p, "nexthdr+", strlen("nexthdr+")) == 0) {
 210                 *offmask = -1;
 211                 p += strlen("nexthdr+");
 212         } else if (matches(*argv, "nexthdr+") == 0) {
 213                 NEXT_ARG();
 214                 *offmask = -1;
 215                 p = *argv;
 216         }
 217
 218         if (get_integer(off, p, 0))
 219                 return -1;
 220         argc--; argv++;
 221
 222         *argc_p = argc;
 223         *argv_p = argv;
 224         return 0;
 225 }
 226
 227
 228 static int parse_u32(int *argc_p, char ***argv_p, struct tc_u32_sel *sel,
 229                      int off, int offmask)
 230 {
 231         int res = -1;
 232         int argc = *argc_p;
 233         char **argv = *argv_p;
 234         __u32 key;
 235         __u32 mask;
 236
 237         if (argc < 2)
 238                 return -1;
 239
 240         if (get_u32(&key, *argv, 0))
 241                 return -1;
 242         argc--; argv++;
 243
 244         if (get_u32(&mask, *argv, 16))
 245                 return -1;
 246         argc--; argv++;
 247
 248         if (argc > 0 && strcmp(argv[0], "at") == 0) {
 249                 NEXT_ARG();
 250                 if (parse_at(&argc, &argv, &off, &offmask))
 251                         return -1;
 252         }
 253
 254         res = pack_key32(sel, key, mask, off, offmask);
 255         *argc_p = argc;
 256         *argv_p = argv;
 257         return res;
 258 }
 259
 260 static int parse_u16(int *argc_p, char ***argv_p, struct tc_u32_sel *sel,
 261                      int off, int offmask)
 262 {
 263         int res = -1;
 264         int argc = *argc_p;
 265         char **argv = *argv_p;
 266         __u32 key;
 267         __u32 mask;
 268
 269         if (argc < 2)
 270                 return -1;
 271
 272         if (get_u32(&key, *argv, 0))
 273                 return -1;
 274         argc--; argv++;
 275
 276         if (get_u32(&mask, *argv, 16))
 277                 return -1;
 278         argc--; argv++;
 279
 280         if (argc > 0 && strcmp(argv[0], "at") == 0) {
 281                 NEXT_ARG();
 282                 if (parse_at(&argc, &argv, &off, &offmask))
 283                         return -1;
 284         }
 285         res = pack_key16(sel, key, mask, off, offmask);
 286         *argc_p = argc;
 287         *argv_p = argv;
 288         return res;
 289 }
 290
 291 static int parse_u8(int *argc_p, char ***argv_p, struct tc_u32_sel *sel,
 292                     int off, int offmask)
 293 {
 294         int res = -1;
 295         int argc = *argc_p;
 296         char **argv = *argv_p;
 297         __u32 key;
 298         __u32 mask;
 299
 300         if (argc < 2)
 301                 return -1;
 302
 303         if (get_u32(&key, *argv, 0))
 304                 return -1;
 305         argc--; argv++;
 306
 307         if (get_u32(&mask, *argv, 16))
 308                 return -1;
 309         argc--; argv++;
 310
 311         if (key > 0xFF || mask > 0xFF)
 312                 return -1;
 313
 314         if (argc > 0 && strcmp(argv[0], "at") == 0) {
 315                 NEXT_ARG();
 316                 if (parse_at(&argc, &argv, &off, &offmask))
 317                         return -1;
 318         }
 319
 320         res = pack_key8(sel, key, mask, off, offmask);
 321         *argc_p = argc;
 322         *argv_p = argv;
 323         return res;
 324 }
 325
 326 static int parse_ip_addr(int *argc_p, char ***argv_p, struct tc_u32_sel *sel,
 327                          int off)
 328 {
 329         int res = -1;
 330         int argc = *argc_p;
 331         char **argv = *argv_p;
 332         inet_prefix addr;
 333         __u32 mask;
 334         int offmask = 0;
 335
 336         if (argc < 1)
 337                 return -1;
 338
 339         if (get_prefix_1(&addr, *argv, AF_INET))
 340                 return -1;
 341         argc--; argv++;
 342
 343         if (argc > 0 && strcmp(argv[0], "at") == 0) {
 344                 NEXT_ARG();
 345                 if (parse_at(&argc, &argv, &off, &offmask))
 346                         return -1;
 347         }
 348
 349         mask = 0;
 350         if (addr.bitlen)
 351                 mask = htonl(0xFFFFFFFF<<(32-addr.bitlen));
 352         if (pack_key(sel, addr.data[0], mask, off, offmask) < 0)
 353                 return -1;
 354         res = 0;
 355
 356         *argc_p = argc;
 357         *argv_p = argv;
 358         return res;
 359 }
 360
 361 static int parse_ip6_addr(int *argc_p, char ***argv_p,
 362                           struct tc_u32_sel *sel, int off)
 363 {
 364         int res = -1;
 365         int argc = *argc_p;
 366         char **argv = *argv_p;
 367         int plen = 128;
 368         int i;
 369         inet_prefix addr;
 370         int offmask = 0;
 371
 372         if (argc < 1)
 373                 return -1;
 374
 375         if (get_prefix_1(&addr, *argv, AF_INET6))
 376                 return -1;
 377         argc--; argv++;
 378
 379         if (argc > 0 && strcmp(argv[0], "at") == 0) {
 380                 NEXT_ARG();
 381                 if (parse_at(&argc, &argv, &off, &offmask))
 382                         return -1;
 383         }
 384
 385         plen = addr.bitlen;
 386         for (i=0; i<plen; i+=32) {
 387 //              if (((i+31)&~0x1F)<=plen) {
 388                 if (i + 31 <= plen) {
 389                         res = pack_key(sel, addr.data[i/32],
 390                                        0xFFFFFFFF, off+4*(i/32), offmask);
 391                         if (res < 0)
 392                                 return -1;
 393                 } else if (i < plen) {
 394                         __u32 mask = htonl(0xFFFFFFFF << (32 - (plen -i )));
 395                         res = pack_key(sel, addr.data[i/32],
 396                                        mask, off+4*(i/32), offmask);
 397                         if (res < 0)
 398                                 return -1;
 399                 }
 400         }
 401         res = 0;
 402
 403         *argc_p = argc;
 404         *argv_p = argv;
 405         return res;
 406 }
 407
 408 static int parse_ip(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
 409 {
 410         int res = -1;
 411         int argc = *argc_p;
 412         char **argv = *argv_p;
 413
 414         if (argc < 2)
 415                 return -1;
 416
 417         if (strcmp(*argv, "src") == 0) {
 418                 NEXT_ARG();
 419                 res = parse_ip_addr(&argc, &argv, sel, 12);
 420                 goto done;
 421         }
 422         if (strcmp(*argv, "dst") == 0) {
 423                 NEXT_ARG();
 424                 res = parse_ip_addr(&argc, &argv, sel, 16);
 425                 goto done;
 426         }
 427         if (strcmp(*argv, "tos") == 0 ||
 428             matches(*argv, "dsfield") == 0) {
 429                 NEXT_ARG();
 430                 res = parse_u8(&argc, &argv, sel, 1, 0);
 431                 goto done;
 432         }
 433         if (strcmp(*argv, "ihl") == 0) {
 434                 NEXT_ARG();
 435                 res = parse_u8(&argc, &argv, sel, 0, 0);
 436                 goto done;
 437         }
 438         if (strcmp(*argv, "protocol") == 0) {
 439                 NEXT_ARG();
 440                 res = parse_u8(&argc, &argv, sel, 9, 0);
 441                 goto done;
 442         }
 443         if (matches(*argv, "precedence") == 0) {
 444                 NEXT_ARG();
 445                 res = parse_u8(&argc, &argv, sel, 1, 0);
 446                 goto done;
 447         }
 448         if (strcmp(*argv, "nofrag") == 0) {
 449                 argc--; argv++;
 450                 res = pack_key16(sel, 0, 0x3FFF, 6, 0);
 451                 goto done;
 452         }
 453         if (strcmp(*argv, "firstfrag") == 0) {
 454                 argc--; argv++;
 455                 res = pack_key16(sel, 0, 0x1FFF, 6, 0);
 456                 goto done;
 457         }
 458         if (strcmp(*argv, "df") == 0) {
 459                 argc--; argv++;
 460                 res = pack_key16(sel, 0x4000, 0x4000, 6, 0);
 461                 goto done;
 462         }
 463         if (strcmp(*argv, "mf") == 0) {
 464                 argc--; argv++;
 465                 res = pack_key16(sel, 0x2000, 0x2000, 6, 0);
 466                 goto done;
 467         }
 468         if (strcmp(*argv, "dport") == 0) {
 469                 NEXT_ARG();
 470                 res = parse_u16(&argc, &argv, sel, 22, 0);
 471                 goto done;
 472         }
 473         if (strcmp(*argv, "sport") == 0) {
 474                 NEXT_ARG();
 475                 res = parse_u16(&argc, &argv, sel, 20, 0);
 476                 goto done;
 477         }
 478         if (strcmp(*argv, "icmp_type") == 0) {
 479                 NEXT_ARG();
 480                 res = parse_u8(&argc, &argv, sel, 20, 0);
 481                 goto done;
 482         }
 483         if (strcmp(*argv, "icmp_code") == 0) {
 484                 NEXT_ARG();
 485                 res = parse_u8(&argc, &argv, sel, 20, 1);
 486                 goto done;
 487         }
 488         return -1;
 489
 490 done:
 491         *argc_p = argc;
 492         *argv_p = argv;
 493         return res;
 494 }
 495
 496 static int parse_ip6(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
 497 {
 498         int res = -1;
 499         int argc = *argc_p;
 500         char **argv = *argv_p;
 501
 502         if (argc < 2)
 503                 return -1;
 504
 505         if (strcmp(*argv, "src") == 0) {
 506                 NEXT_ARG();
 507                 res = parse_ip6_addr(&argc, &argv, sel, 8);
 508                 goto done;
 509         }
 510         if (strcmp(*argv, "dst") == 0) {
 511                 NEXT_ARG();
 512                 res = parse_ip6_addr(&argc, &argv, sel, 24);
 513                 goto done;
 514         }
 515         if (strcmp(*argv, "priority") == 0) {
 516                 NEXT_ARG();
 517                 res = parse_u8(&argc, &argv, sel, 4, 0);
 518                 goto done;
 519         }
 520         if (strcmp(*argv, "protocol") == 0) {
 521                 NEXT_ARG();
 522                 res = parse_u8(&argc, &argv, sel, 6, 0);
 523                 goto done;
 524         }
 525         if (strcmp(*argv, "flowlabel") == 0) {
 526                 NEXT_ARG();
 527                 res = parse_u32(&argc, &argv, sel, 0, 0);
 528                 goto done;
 529         }
 530         if (strcmp(*argv, "dport") == 0) {
 531                 NEXT_ARG();
 532                 res = parse_u16(&argc, &argv, sel, 42, 0);
 533                 goto done;
 534         }
 535         if (strcmp(*argv, "sport") == 0) {
 536                 NEXT_ARG();
 537                 res = parse_u16(&argc, &argv, sel, 40, 0);
 538                 goto done;
 539         }
 540         if (strcmp(*argv, "icmp_type") == 0) {
 541                 NEXT_ARG();
 542                 res = parse_u8(&argc, &argv, sel, 40, 0);
 543                 goto done;
 544         }
 545         if (strcmp(*argv, "icmp_code") == 0) {
 546                 NEXT_ARG();
 547                 res = parse_u8(&argc, &argv, sel, 41, 1);
 548                 goto done;
 549         }
 550         return -1;
 551
 552 done:
 553         *argc_p = argc;
 554         *argv_p = argv;
 555         return res;
 556 }
 557
 558 #define parse_tcp parse_udp
 559 static int parse_udp(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
 560 {
 561         int res = -1;
 562         int argc = *argc_p;
 563         char **argv = *argv_p;
 564
 565         if (argc < 2)
 566                 return -1;
 567
 568         if (strcmp(*argv, "src") == 0) {
 569                 NEXT_ARG();
 570                 res = parse_u16(&argc, &argv, sel, 0, -1);
 571                 goto done;
 572         }
 573         if (strcmp(*argv, "dst") == 0) {
 574                 NEXT_ARG();
 575                 res = parse_u16(&argc, &argv, sel, 2, -1);
 576                 goto done;
 577         }
 578         return -1;
 579
 580 done:
 581         *argc_p = argc;
 582         *argv_p = argv;
 583         return res;
 584 }
 585
 586
 587 static int parse_icmp(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
 588 {
 589         int res = -1;
 590         int argc = *argc_p;
 591         char **argv = *argv_p;
 592
 593         if (argc < 2)
 594                 return -1;
 595
 596         if (strcmp(*argv, "type") == 0) {
 597                 NEXT_ARG();
 598                 res = parse_u8(&argc, &argv, sel, 0, -1);
 599                 goto done;
 600         }
 601         if (strcmp(*argv, "code") == 0) {
 602                 NEXT_ARG();
 603                 res = parse_u8(&argc, &argv, sel, 1, -1);
 604                 goto done;
 605         }
 606         return -1;
 607
 608 done:
 609         *argc_p = argc;
 610         *argv_p = argv;
 611         return res;
 612 }
 613
 614 static int parse_mark(int *argc_p, char ***argv_p, struct nlmsghdr *n)
 615 {
 616         int res = -1;
 617         int argc = *argc_p;
 618         char **argv = *argv_p;
 619         struct tc_u32_mark mark;
 620
 621         if (argc <= 1)
 622                 return -1;
 623
 624         if (get_u32(&mark.val, *argv, 0)) {
 625                 fprintf(stderr, "Illegal \"mark\" value\n");
 626                 return -1;
 627         }
 628         NEXT_ARG();
 629
 630         if (get_u32(&mark.mask, *argv, 0)) {
 631                 fprintf(stderr, "Illegal \"mark\" mask\n");
 632                 return -1;
 633         }
 634         NEXT_ARG();
 635
 636         if ((mark.val & mark.mask) != mark.val) {
 637                 fprintf(stderr, "Illegal \"mark\" (impossible combination)\n");
 638                 return -1;
 639         }
 640
 641         addattr_l(n, MAX_MSG, TCA_U32_MARK, &mark, sizeof(mark));
 642         res = 0;
 643
 644         *argc_p = argc;
 645         *argv_p = argv;
 646         return res;
 647 }
 648
 649 static int parse_selector(int *argc_p, char ***argv_p,
 650                           struct tc_u32_sel *sel, struct nlmsghdr *n)
 651 {
 652         int argc = *argc_p;
 653         char **argv = *argv_p;
 654         int res = -1;
 655
 656         if (argc <= 0)
 657                 return -1;
 658
 659         if (matches(*argv, "u32") == 0) {
 660                 NEXT_ARG();
 661                 res = parse_u32(&argc, &argv, sel, 0, 0);
 662                 goto done;
 663         }
 664         if (matches(*argv, "u16") == 0) {
 665                 NEXT_ARG();
 666                 res = parse_u16(&argc, &argv, sel, 0, 0);
 667                 goto done;
 668         }
 669         if (matches(*argv, "u8") == 0) {
 670                 NEXT_ARG();
 671                 res = parse_u8(&argc, &argv, sel, 0, 0);
 672                 goto done;
 673         }
 674         if (matches(*argv, "ip") == 0) {
 675                 NEXT_ARG();
 676                 res = parse_ip(&argc, &argv, sel);
 677                 goto done;
 678         }
 679         if (matches(*argv, "ip6") == 0) {
 680                 NEXT_ARG();
 681                 res = parse_ip6(&argc, &argv, sel);
 682                 goto done;
 683         }
 684         if (matches(*argv, "udp") == 0) {
 685                 NEXT_ARG();
 686                 res = parse_udp(&argc, &argv, sel);
 687                 goto done;
 688         }
 689         if (matches(*argv, "tcp") == 0) {
 690                 NEXT_ARG();
 691                 res = parse_tcp(&argc, &argv, sel);
 692                 goto done;
 693         }
 694         if (matches(*argv, "icmp") == 0) {
 695                 NEXT_ARG();
 696                 res = parse_icmp(&argc, &argv, sel);
 697                 goto done;
 698         }
 699         if (matches(*argv, "mark") == 0) {
 700                 NEXT_ARG();
 701                 res = parse_mark(&argc, &argv, n);
 702                 goto done;
 703         }
 704
 705         return -1;
 706
 707 done:
 708         *argc_p = argc;
 709         *argv_p = argv;
 710         return res;
 711 }
 712
 713 static int parse_offset(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
 714 {
 715         int argc = *argc_p;
 716         char **argv = *argv_p;
 717
 718         while (argc > 0) {
 719                 if (matches(*argv, "plus") == 0) {
 720                         int off;
 721                         NEXT_ARG();
 722                         if (get_integer(&off, *argv, 0))
 723                                 return -1;
 724                         sel->off = off;
 725                         sel->flags |= TC_U32_OFFSET;
 726                 } else if (matches(*argv, "at") == 0) {
 727                         int off;
 728                         NEXT_ARG();
 729                         if (get_integer(&off, *argv, 0))
 730                                 return -1;
 731                         sel->offoff = off;
 732                         if (off%2) {
 733                                 fprintf(stderr, "offset \"at\" must be even\n");
 734                                 return -1;
 735                         }
 736                         sel->flags |= TC_U32_VAROFFSET;
 737                 } else if (matches(*argv, "mask") == 0) {
 738                         __u16 mask;
 739                         NEXT_ARG();
 740                         if (get_u16(&mask, *argv, 16))
 741                                 return -1;
 742                         sel->offmask = htons(mask);
 743                         sel->flags |= TC_U32_VAROFFSET;
 744                 } else if (matches(*argv, "shift") == 0) {
 745                         int shift;
 746                         NEXT_ARG();
 747                         if (get_integer(&shift, *argv, 0))
 748                                 return -1;
 749                         sel->offshift = shift;
 750                         sel->flags |= TC_U32_VAROFFSET;
 751                 } else if (matches(*argv, "eat") == 0) {
 752                         sel->flags |= TC_U32_EAT;
 753                 } else {
 754                         break;
 755                 }
 756                 argc--; argv++;
 757         }
 758
 759         *argc_p = argc;
 760         *argv_p = argv;
 761         return 0;
 762 }
 763
 764 static int parse_hashkey(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
 765 {
 766         int argc = *argc_p;
 767         char **argv = *argv_p;
 768
 769         while (argc > 0) {
 770                 if (matches(*argv, "mask") == 0) {
 771                         __u32 mask;
 772                         NEXT_ARG();
 773                         if (get_u32(&mask, *argv, 16))
 774                                 return -1;
 775                         sel->hmask = htonl(mask);
 776                 } else if (matches(*argv, "at") == 0) {
 777                         int num;
 778                         NEXT_ARG();
 779                         if (get_integer(&num, *argv, 0))
 780                                 return -1;
 781                         if (num%4)
 782                                 return -1;
 783                         sel->hoff = num;
 784                 } else {
 785                         break;
 786                 }
 787                 argc--; argv++;
 788         }
 789
 790         *argc_p = argc;
 791         *argv_p = argv;
 792         return 0;
 793 }
 794
 795 static void print_ipv4(FILE *f, const struct tc_u32_key *key)
 796 {
 797         char abuf[256];
 798
 799         switch (key->off) {
 800         case 0:
 801                 switch (ntohl(key->mask)) {
 802                 case 0x0f000000:
 803                         fprintf(f, "\n  match IP ihl %u", ntohl(key->val) >> 24);
 804                         return;
 805                 case 0x00ff0000:
 806                         fprintf(f, "\n  match IP dsfield %#x", ntohl(key->val) >> 16);
 807                         return;
 808                 }
 809                 break;
 810         case 8:
 811                 if (ntohl(key->mask) == 0x00ff0000) {
 812                         fprintf(f, "\n  match IP protocol %d", ntohl(key->val) >> 16);
 813                         return;
 814                 }
 815                 break;
 816         case 12:
 817         case 16: {
 818                         int bits = mask2bits(key->mask);
 819                         if (bits >= 0) {
 820                                 fprintf(f, "\n  %s %s/%d",
 821                                         key->off == 12 ? "match IP src" : "match IP dst",
 822                                         inet_ntop(AF_INET, &key->val,
 823                                                   abuf, sizeof(abuf)),
 824                                         bits);
 825                                 return;
 826                         }
 827                 }
 828                 break;
 829
 830         case 20:
 831                 switch (ntohl(key->mask)) {
 832                 case 0x0000ffff:
 833                         fprintf(f, "\n  match sport %u",
 834                                 ntohl(key->val) & 0xffff);
 835                         return;
 836                 case 0xffff0000:
 837                         fprintf(f, "\n  match dport %u",
 838                                 ntohl(key->val) >> 16);
 839                         return;
 840                 case 0xffffffff:
 841                         fprintf(f, "\n  match sport %u, match dport %u",
 842                                 ntohl(key->val) & 0xffff,
 843                                 ntohl(key->val) >> 16);
 844
 845                         return;
 846                 }
 847                 /* XXX: Default print_raw */
 848         }
 849 }
 850
 851 static void print_raw(FILE *f, const struct tc_u32_key *key)
 852 {
 853         fprintf(f, "\n  match %08x/%08x at %s%d",
 854                 (unsigned int)ntohl(key->val),
 855                 (unsigned int)ntohl(key->mask),
 856                 key->offmask ? "nexthdr+" : "",
 857                 key->off);
 858 }
 859
 860 static const struct {
 861         __u16 proto;
 862         __u16 pad;
 863         void (*pprinter)(FILE *f, const struct tc_u32_key *key);
 864 } u32_pprinters[] = {
 865         {0,        0, print_raw},
 866         {ETH_P_IP, 0, print_ipv4},
 867 };
 868
 869 static void show_keys(FILE *f, const struct tc_u32_key *key)
 870 {
 871         int i = 0;
 872
 873         if (!show_pretty)
 874                 goto show_k;
 875
 876         for (i = 0; i < sizeof(u32_pprinters) / sizeof(u32_pprinters[0]); i++) {
 877                 if (u32_pprinters[i].proto == ntohs(f_proto)) {
 878 show_k:
 879                         u32_pprinters[i].pprinter(f, key);
 880                         return;
 881                 }
 882         }
 883
 884         i = 0;
 885         goto show_k;
 886 }
 887
 888 static int u32_parse_opt(struct filter_util *qu, char *handle,
 889                          int argc, char **argv, struct nlmsghdr *n)
 890 {
 891         struct {
 892                 struct tc_u32_sel sel;
 893                 struct tc_u32_key keys[128];
 894         } sel;
 895         struct tcmsg *t = NLMSG_DATA(n);
 896         struct rtattr *tail;
 897         int sel_ok = 0, terminal_ok = 0;
 898         int sample_ok = 0;
 899         __u32 htid = 0;
 900         __u32 order = 0;
 901
 902         memset(&sel, 0, sizeof(sel));
 903
 904         if (handle && get_u32_handle(&t->tcm_handle, handle)) {
 905                 fprintf(stderr, "Illegal filter ID\n");
 906                 return -1;
 907         }
 908
 909         if (argc == 0)
 910                 return 0;
 911
 912         tail = NLMSG_TAIL(n);
 913         addattr_l(n, MAX_MSG, TCA_OPTIONS, NULL, 0);
 914
 915         while (argc > 0) {
 916                 if (matches(*argv, "match") == 0) {
 917                         NEXT_ARG();
 918                         if (parse_selector(&argc, &argv, &sel.sel, n)) {
 919                                 fprintf(stderr, "Illegal \"match\"\n");
 920                                 return -1;
 921                         }
 922                         sel_ok++;
 923                         continue;
 924                 } else if (matches(*argv, "offset") == 0) {
 925                         NEXT_ARG();
 926                         if (parse_offset(&argc, &argv, &sel.sel)) {
 927                                 fprintf(stderr, "Illegal \"offset\"\n");
 928                                 return -1;
 929                         }
 930                         continue;
 931                 } else if (matches(*argv, "hashkey") == 0) {
 932                         NEXT_ARG();
 933                         if (parse_hashkey(&argc, &argv, &sel.sel)) {
 934                                 fprintf(stderr, "Illegal \"hashkey\"\n");
 935                                 return -1;
 936                         }
 937                         continue;
 938                 } else if (matches(*argv, "classid") == 0 ||
 939                            strcmp(*argv, "flowid") == 0) {
 940                         unsigned handle;
 941                         NEXT_ARG();
 942                         if (get_tc_classid(&handle, *argv)) {
 943                                 fprintf(stderr, "Illegal \"classid\"\n");
 944                                 return -1;
 945                         }
 946                         addattr_l(n, MAX_MSG, TCA_U32_CLASSID, &handle, 4);
 947                         sel.sel.flags |= TC_U32_TERMINAL;
 948                 } else if (matches(*argv, "divisor") == 0) {
 949                         unsigned divisor;
 950                         NEXT_ARG();
 951                         if (get_unsigned(&divisor, *argv, 0) ||
 952                             divisor == 0 ||
 953                             divisor > 0x100 || ((divisor - 1) & divisor)) {
 954                                 fprintf(stderr, "Illegal \"divisor\"\n");
 955                                 return -1;
 956                         }
 957                         addattr_l(n, MAX_MSG, TCA_U32_DIVISOR, &divisor, 4);
 958                 } else if (matches(*argv, "order") == 0) {
 959                         NEXT_ARG();
 960                         if (get_u32(&order, *argv, 0)) {
 961                                 fprintf(stderr, "Illegal \"order\"\n");
 962                                 return -1;
 963                         }
 964                 } else if (strcmp(*argv, "link") == 0) {
 965                         unsigned handle;
 966                         NEXT_ARG();
 967                         if (get_u32_handle(&handle, *argv)) {
 968                                 fprintf(stderr, "Illegal \"link\"\n");
 969                                 return -1;
 970                         }
 971                         if (handle && TC_U32_NODE(handle)) {
 972                                 fprintf(stderr, "\"link\" must be a hash table.\n");
 973                                 return -1;
 974                         }
 975                         addattr_l(n, MAX_MSG, TCA_U32_LINK, &handle, 4);
 976                 } else if (strcmp(*argv, "ht") == 0) {
 977                         unsigned handle;
 978                         NEXT_ARG();
 979                         if (get_u32_handle(&handle, *argv)) {
 980                                 fprintf(stderr, "Illegal \"ht\"\n");
 981                                 return -1;
 982                         }
 983                         if (handle && TC_U32_NODE(handle)) {
 984                                 fprintf(stderr, "\"ht\" must be a hash table.\n");
 985                                 return -1;
 986                         }
 987                         if (sample_ok)
 988                                 htid = (htid&0xFF000)|(handle&0xFFF00000);
 989                         else
 990                                 htid = (handle&0xFFFFF000);
 991                 } else if (strcmp(*argv, "sample") == 0) {
 992                         __u32 hash;
 993                         unsigned divisor = 0x100;
 994
 995                         struct {
 996                                 struct tc_u32_sel sel;
 997                                 struct tc_u32_key keys[4];
 998                         } sel2;
 999                         memset(&sel2, 0, sizeof(sel2));
1000                         NEXT_ARG();
1001                         if (parse_selector(&argc, &argv, &sel2.sel, n)) {
1002                                 fprintf(stderr, "Illegal \"sample\"\n");
1003                                 return -1;
1004                         }
1005                         if (sel2.sel.nkeys != 1) {
1006                                 fprintf(stderr, "\"sample\" must contain"
1007                                         " exactly ONE key.\n");
1008                                 return -1;
1009                         }
1010                         if (*argv != 0 && strcmp(*argv, "divisor") == 0) {
1011                                 NEXT_ARG();
1012                                 if (get_unsigned(&divisor, *argv, 0) || divisor == 0 ||
1013                                     divisor > 0x100 || ((divisor - 1) & divisor)) {
1014                                         fprintf(stderr, "Illegal sample \"divisor\"\n");
1015                                         return -1;
1016                                 }
1017                                 NEXT_ARG();
1018                         }
1019                         hash = sel2.sel.keys[0].val&sel2.sel.keys[0].mask;
1020                         hash ^= hash>>16;
1021                         hash ^= hash>>8;
1022                         htid = ((hash%divisor)<<12)|(htid&0xFFF00000);
1023                         sample_ok = 1;
1024                         continue;
1025                 } else if (strcmp(*argv, "indev") == 0) {
1026                         char ind[IFNAMSIZ + 1];
1027                         memset(ind, 0, sizeof (ind));
1028                         argc--;
1029                         argv++;
1030                         if (argc < 1) {
1031                                 fprintf(stderr, "Illegal indev\n");
1032                                 return -1;
1033                         }
1034                         strncpy(ind, *argv, sizeof (ind) - 1);
1035                         addattr_l(n, MAX_MSG, TCA_U32_INDEV, ind, strlen(ind) + 1);
1036
1037                 } else if (matches(*argv, "action") == 0) {
1038                         NEXT_ARG();
1039                         if (parse_action(&argc, &argv, TCA_U32_ACT, n)) {
1040                                 fprintf(stderr, "Illegal \"action\"\n");
1041                                 return -1;
1042                         }
1043                         terminal_ok++;
1044                         continue;
1045
1046                 } else if (matches(*argv, "police") == 0) {
1047                         NEXT_ARG();
1048                         if (parse_police(&argc, &argv, TCA_U32_POLICE, n)) {
1049                                 fprintf(stderr, "Illegal \"police\"\n");
1050                                 return -1;
1051                         }
1052                         terminal_ok++;
1053                         continue;
1054                 } else if (strcmp(*argv, "help") == 0) {
1055                         explain();
1056                         return -1;
1057                 } else {
1058                         fprintf(stderr, "What is \"%s\"?\n", *argv);
1059                         explain();
1060                         return -1;
1061                 }
1062                 argc--; argv++;
1063         }
1064
1065         /* We dont necessarily need class/flowids */
1066         if (terminal_ok)
1067                 sel.sel.flags |= TC_U32_TERMINAL;
1068
1069         if (order) {
1070                 if (TC_U32_NODE(t->tcm_handle) && order != TC_U32_NODE(t->tcm_handle)) {
1071                         fprintf(stderr, "\"order\" contradicts \"handle\"\n");
1072                         return -1;
1073                 }
1074                 t->tcm_handle |= order;
1075         }
1076
1077         if (htid)
1078                 addattr_l(n, MAX_MSG, TCA_U32_HASH, &htid, 4);
1079         if (sel_ok)
1080                 addattr_l(n, MAX_MSG, TCA_U32_SEL, &sel,
1081                           sizeof(sel.sel)+sel.sel.nkeys*sizeof(struct tc_u32_key));
1082         tail->rta_len = (void *) NLMSG_TAIL(n) - (void *) tail;
1083         return 0;
1084 }
1085
1086 static int u32_print_opt(struct filter_util *qu, FILE *f, struct rtattr *opt,
1087                          __u32 handle)
1088 {
1089         struct rtattr *tb[TCA_U32_MAX+1];
1090         struct tc_u32_sel *sel = NULL;
1091         struct tc_u32_pcnt *pf = NULL;
1092
1093         if (opt == NULL)
1094                 return 0;
1095
1096         parse_rtattr_nested(tb, TCA_U32_MAX, opt);
1097
1098         if (handle) {
1099                 SPRINT_BUF(b1);
1100                 fprintf(f, "fh %s ", sprint_u32_handle(handle, b1));
1101         }
1102         if (TC_U32_NODE(handle)) {
1103                 fprintf(f, "order %d ", TC_U32_NODE(handle));
1104         }
1105
1106         if (tb[TCA_U32_SEL]) {
1107                 if (RTA_PAYLOAD(tb[TCA_U32_SEL])  < sizeof(*sel))
1108                         return -1;
1109
1110                 sel = RTA_DATA(tb[TCA_U32_SEL]);
1111         }
1112
1113         if (tb[TCA_U32_DIVISOR]) {
1114                 fprintf(f, "ht divisor %d ", *(__u32*)RTA_DATA(tb[TCA_U32_DIVISOR]));
1115         } else if (tb[TCA_U32_HASH]) {
1116                 __u32 htid = *(__u32*)RTA_DATA(tb[TCA_U32_HASH]);
1117                 fprintf(f, "key ht %x bkt %x ", TC_U32_USERHTID(htid),
1118                         TC_U32_HASH(htid));
1119         } else {
1120                 fprintf(f, "??? ");
1121         }
1122         if (tb[TCA_U32_CLASSID]) {
1123                 SPRINT_BUF(b1);
1124                 fprintf(f, "%sflowid %s ",
1125                         !sel || !(sel->flags&TC_U32_TERMINAL) ? "*" : "",
1126                         sprint_tc_classid(*(__u32*)RTA_DATA(tb[TCA_U32_CLASSID]), b1));
1127         } else if (sel && sel->flags&TC_U32_TERMINAL) {
1128                 fprintf(f, "terminal flowid ??? ");
1129         }
1130         if (tb[TCA_U32_LINK]) {
1131                 SPRINT_BUF(b1);
1132                 fprintf(f, "link %s ",
1133                         sprint_u32_handle(*(__u32*)RTA_DATA(tb[TCA_U32_LINK]), b1));
1134         }
1135
1136         if (tb[TCA_U32_PCNT]) {
1137                 if (RTA_PAYLOAD(tb[TCA_U32_PCNT])  < sizeof(*pf)) {
1138                         fprintf(f, "Broken perf counters \n");
1139                         return -1;
1140                 }
1141                 pf = RTA_DATA(tb[TCA_U32_PCNT]);
1142         }
1143
1144         if (sel && show_stats && NULL != pf)
1145                 fprintf(f, " (rule hit %llu success %llu)",
1146                         (unsigned long long) pf->rcnt,
1147                         (unsigned long long) pf->rhit);
1148
1149         if (tb[TCA_U32_MARK]) {
1150                 struct tc_u32_mark *mark = RTA_DATA(tb[TCA_U32_MARK]);
1151                 if (RTA_PAYLOAD(tb[TCA_U32_MARK]) < sizeof(*mark)) {
1152                         fprintf(f, "\n  Invalid mark (kernel&iproute2 mismatch)\n");
1153                 } else {
1154                         fprintf(f, "\n  mark 0x%04x 0x%04x (success %d)",
1155                                 mark->val, mark->mask, mark->success);
1156                 }
1157         }
1158
1159         if (sel) {
1160                 if (sel->nkeys) {
1161                         int i;
1162                         for (i=0; i<sel->nkeys; i++) {
1163                                 show_keys(f, sel->keys + i);
1164                                 if (show_stats && NULL != pf)
1165                                         fprintf(f, " (success %llu ) ",
1166                                                 (unsigned long long) pf->kcnts[i]);
1167                         }
1168                 }
1169
1170                 if (sel->flags&(TC_U32_VAROFFSET|TC_U32_OFFSET)) {
1171                         fprintf(f, "\n    offset ");
1172                         if (sel->flags&TC_U32_VAROFFSET)
1173                                 fprintf(f, "%04x>>%d at %d ",
1174                                         ntohs(sel->offmask),
1175                                         sel->offshift,  sel->offoff);
1176                         if (sel->off)
1177                                 fprintf(f, "plus %d ", sel->off);
1178                 }
1179                 if (sel->flags&TC_U32_EAT)
1180                         fprintf(f, " eat ");
1181
1182                 if (sel->hmask) {
1183                         fprintf(f, "\n    hash mask %08x at %d ",
1184                                 (unsigned int)htonl(sel->hmask), sel->hoff);
1185                 }
1186         }
1187
1188         if (tb[TCA_U32_POLICE]) {
1189                 fprintf(f, "\n");
1190                 tc_print_police(f, tb[TCA_U32_POLICE]);
1191         }
1192         if (tb[TCA_U32_INDEV]) {
1193                 struct rtattr *idev = tb[TCA_U32_INDEV];
1194                 fprintf(f, "\n  input dev %s\n", (char *) RTA_DATA(idev));
1195         }
1196         if (tb[TCA_U32_ACT]) {
1197                 tc_print_action(f, tb[TCA_U32_ACT]);
1198         }
1199
1200         return 0;
1201 }
1202
1203 struct filter_util u32_filter_util = {
1204         .id = "u32",
1205         .parse_fopt = u32_parse_opt,
1206         .print_fopt = u32_print_opt,
1207 };