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