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, const 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_ether_addr(int *argc_p, char ***argv_p,
 409                             struct tc_u32_sel *sel, int off)
 410 {
 411         int res = -1;
 412         int argc = *argc_p;
 413         char **argv = *argv_p;
 414         __u8 addr[6];
 415         int offmask = 0;
 416         __u32 key;
 417         int i;
 418
 419         if (argc < 1)
 420                 return -1;
 421
 422         if (sscanf(*argv, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
 423                    addr + 0, addr + 1, addr + 2,
 424                    addr + 3, addr + 4, addr + 5) != 6) {
 425                 fprintf(stderr, "parse_ether_addr: improperly formed address '%s'\n",
 426                         *argv);
 427                 return -1;
 428         }
 429
 430         argc--; argv++;
 431         if (argc > 0 && strcmp(argv[0], "at") == 0) {
 432                 NEXT_ARG();
 433                 if (parse_at(&argc, &argv, &off, &offmask))
 434                         return -1;
 435         }
 436
 437         for (i = 0; i < 6; i += 2) {
 438                 key = *(__u16 *) (addr + i);
 439
 440                 res = pack_key16(sel, key, 0xFFFF, off + i, offmask);
 441                 if (res < 0)
 442                         return -1;
 443         }
 444
 445         *argc_p = argc;
 446         *argv_p = argv;
 447         return res;
 448 }
 449
 450 static int parse_ip(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
 451 {
 452         int res = -1;
 453         int argc = *argc_p;
 454         char **argv = *argv_p;
 455
 456         if (argc < 2)
 457                 return -1;
 458
 459         if (strcmp(*argv, "src") == 0) {
 460                 NEXT_ARG();
 461                 res = parse_ip_addr(&argc, &argv, sel, 12);
 462         } else if (strcmp(*argv, "dst") == 0) {
 463                 NEXT_ARG();
 464                 res = parse_ip_addr(&argc, &argv, sel, 16);
 465         } else if (strcmp(*argv, "tos") == 0 ||
 466             matches(*argv, "dsfield") == 0) {
 467                 NEXT_ARG();
 468                 res = parse_u8(&argc, &argv, sel, 1, 0);
 469         } else if (strcmp(*argv, "ihl") == 0) {
 470                 NEXT_ARG();
 471                 res = parse_u8(&argc, &argv, sel, 0, 0);
 472         } else if (strcmp(*argv, "protocol") == 0) {
 473                 NEXT_ARG();
 474                 res = parse_u8(&argc, &argv, sel, 9, 0);
 475         } else if (matches(*argv, "precedence") == 0) {
 476                 NEXT_ARG();
 477                 res = parse_u8(&argc, &argv, sel, 1, 0);
 478         } else if (strcmp(*argv, "nofrag") == 0) {
 479                 argc--; argv++;
 480                 res = pack_key16(sel, 0, 0x3FFF, 6, 0);
 481         } else if (strcmp(*argv, "firstfrag") == 0) {
 482                 argc--; argv++;
 483                 res = pack_key16(sel, 0, 0x1FFF, 6, 0);
 484         } else if (strcmp(*argv, "df") == 0) {
 485                 argc--; argv++;
 486                 res = pack_key16(sel, 0x4000, 0x4000, 6, 0);
 487         } else if (strcmp(*argv, "mf") == 0) {
 488                 argc--; argv++;
 489                 res = pack_key16(sel, 0x2000, 0x2000, 6, 0);
 490         } else if (strcmp(*argv, "dport") == 0) {
 491                 NEXT_ARG();
 492                 res = parse_u16(&argc, &argv, sel, 22, 0);
 493         } else if (strcmp(*argv, "sport") == 0) {
 494                 NEXT_ARG();
 495                 res = parse_u16(&argc, &argv, sel, 20, 0);
 496         } else if (strcmp(*argv, "icmp_type") == 0) {
 497                 NEXT_ARG();
 498                 res = parse_u8(&argc, &argv, sel, 20, 0);
 499         } else if (strcmp(*argv, "icmp_code") == 0) {
 500                 NEXT_ARG();
 501                 res = parse_u8(&argc, &argv, sel, 20, 1);
 502         } else
 503                 return -1;
 504
 505         *argc_p = argc;
 506         *argv_p = argv;
 507         return res;
 508 }
 509
 510 static int parse_ip6(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
 511 {
 512         int res = -1;
 513         int argc = *argc_p;
 514         char **argv = *argv_p;
 515
 516         if (argc < 2)
 517                 return -1;
 518
 519         if (strcmp(*argv, "src") == 0) {
 520                 NEXT_ARG();
 521                 res = parse_ip6_addr(&argc, &argv, sel, 8);
 522         } else if (strcmp(*argv, "dst") == 0) {
 523                 NEXT_ARG();
 524                 res = parse_ip6_addr(&argc, &argv, sel, 24);
 525         } else if (strcmp(*argv, "priority") == 0) {
 526                 NEXT_ARG();
 527                 res = parse_u8(&argc, &argv, sel, 4, 0);
 528         } else if (strcmp(*argv, "protocol") == 0) {
 529                 NEXT_ARG();
 530                 res = parse_u8(&argc, &argv, sel, 6, 0);
 531         } else if (strcmp(*argv, "flowlabel") == 0) {
 532                 NEXT_ARG();
 533                 res = parse_u32(&argc, &argv, sel, 0, 0);
 534         } else if (strcmp(*argv, "dport") == 0) {
 535                 NEXT_ARG();
 536                 res = parse_u16(&argc, &argv, sel, 42, 0);
 537         } else if (strcmp(*argv, "sport") == 0) {
 538                 NEXT_ARG();
 539                 res = parse_u16(&argc, &argv, sel, 40, 0);
 540         } else if (strcmp(*argv, "icmp_type") == 0) {
 541                 NEXT_ARG();
 542                 res = parse_u8(&argc, &argv, sel, 40, 0);
 543         } else if (strcmp(*argv, "icmp_code") == 0) {
 544                 NEXT_ARG();
 545                 res = parse_u8(&argc, &argv, sel, 41, 1);
 546         } else
 547                 return -1;
 548
 549         *argc_p = argc;
 550         *argv_p = argv;
 551         return res;
 552 }
 553
 554 static int parse_ether(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
 555 {
 556         int res = -1;
 557         int argc = *argc_p;
 558         char **argv = *argv_p;
 559
 560         if (argc < 2)
 561                 return -1;
 562
 563         if (strcmp(*argv, "src") == 0) {
 564                 NEXT_ARG();
 565                 res = parse_ether_addr(&argc, &argv, sel, -8);
 566         } else if (strcmp(*argv, "dst") == 0) {
 567                 NEXT_ARG();
 568                 res = parse_ether_addr(&argc, &argv, sel, -14);
 569         } else {
 570                 fprintf(stderr, "Unknown match: ether %s\n", *argv);
 571                 return -1;
 572         }
 573
 574         *argc_p = argc;
 575         *argv_p = argv;
 576         return res;
 577 }
 578
 579 #define parse_tcp parse_udp
 580 static int parse_udp(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
 581 {
 582         int res = -1;
 583         int argc = *argc_p;
 584         char **argv = *argv_p;
 585
 586         if (argc < 2)
 587                 return -1;
 588
 589         if (strcmp(*argv, "src") == 0) {
 590                 NEXT_ARG();
 591                 res = parse_u16(&argc, &argv, sel, 0, -1);
 592         } else if (strcmp(*argv, "dst") == 0) {
 593                 NEXT_ARG();
 594                 res = parse_u16(&argc, &argv, sel, 2, -1);
 595         } else
 596                 return -1;
 597
 598         *argc_p = argc;
 599         *argv_p = argv;
 600         return res;
 601 }
 602
 603
 604 static int parse_icmp(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
 605 {
 606         int res = -1;
 607         int argc = *argc_p;
 608         char **argv = *argv_p;
 609
 610         if (argc < 2)
 611                 return -1;
 612
 613         if (strcmp(*argv, "type") == 0) {
 614                 NEXT_ARG();
 615                 res = parse_u8(&argc, &argv, sel, 0, -1);
 616         } else if (strcmp(*argv, "code") == 0) {
 617                 NEXT_ARG();
 618                 res = parse_u8(&argc, &argv, sel, 1, -1);
 619         } else
 620                 return -1;
 621
 622         *argc_p = argc;
 623         *argv_p = argv;
 624         return res;
 625 }
 626
 627 static int parse_mark(int *argc_p, char ***argv_p, struct nlmsghdr *n)
 628 {
 629         int res = -1;
 630         int argc = *argc_p;
 631         char **argv = *argv_p;
 632         struct tc_u32_mark mark;
 633
 634         if (argc <= 1)
 635                 return -1;
 636
 637         if (get_u32(&mark.val, *argv, 0)) {
 638                 fprintf(stderr, "Illegal \"mark\" value\n");
 639                 return -1;
 640         }
 641         NEXT_ARG();
 642
 643         if (get_u32(&mark.mask, *argv, 0)) {
 644                 fprintf(stderr, "Illegal \"mark\" mask\n");
 645                 return -1;
 646         }
 647         NEXT_ARG();
 648
 649         if ((mark.val & mark.mask) != mark.val) {
 650                 fprintf(stderr, "Illegal \"mark\" (impossible combination)\n");
 651                 return -1;
 652         }
 653
 654         addattr_l(n, MAX_MSG, TCA_U32_MARK, &mark, sizeof(mark));
 655         res = 0;
 656
 657         *argc_p = argc;
 658         *argv_p = argv;
 659         return res;
 660 }
 661
 662 static int parse_selector(int *argc_p, char ***argv_p,
 663                           struct tc_u32_sel *sel, struct nlmsghdr *n)
 664 {
 665         int argc = *argc_p;
 666         char **argv = *argv_p;
 667         int res = -1;
 668
 669         if (argc <= 0)
 670                 return -1;
 671
 672         if (matches(*argv, "u32") == 0) {
 673                 NEXT_ARG();
 674                 res = parse_u32(&argc, &argv, sel, 0, 0);
 675         } else if (matches(*argv, "u16") == 0) {
 676                 NEXT_ARG();
 677                 res = parse_u16(&argc, &argv, sel, 0, 0);
 678         } else if (matches(*argv, "u8") == 0) {
 679                 NEXT_ARG();
 680                 res = parse_u8(&argc, &argv, sel, 0, 0);
 681         } else if (matches(*argv, "ip") == 0) {
 682                 NEXT_ARG();
 683                 res = parse_ip(&argc, &argv, sel);
 684         } else  if (matches(*argv, "ip6") == 0) {
 685                 NEXT_ARG();
 686                 res = parse_ip6(&argc, &argv, sel);
 687         } else if (matches(*argv, "udp") == 0) {
 688                 NEXT_ARG();
 689                 res = parse_udp(&argc, &argv, sel);
 690         } else if (matches(*argv, "tcp") == 0) {
 691                 NEXT_ARG();
 692                 res = parse_tcp(&argc, &argv, sel);
 693         } else if (matches(*argv, "icmp") == 0) {
 694                 NEXT_ARG();
 695                 res = parse_icmp(&argc, &argv, sel);
 696         } else if (matches(*argv, "mark") == 0) {
 697                 NEXT_ARG();
 698                 res = parse_mark(&argc, &argv, n);
 699         } else if (matches(*argv, "ether") == 0) {
 700                 NEXT_ARG();
 701                 res = parse_ether(&argc, &argv, sel);
 702         } else
 703                 return -1;
 704
 705         *argc_p = argc;
 706         *argv_p = argv;
 707         return res;
 708 }
 709
 710 static int parse_offset(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
 711 {
 712         int argc = *argc_p;
 713         char **argv = *argv_p;
 714
 715         while (argc > 0) {
 716                 if (matches(*argv, "plus") == 0) {
 717                         int off;
 718                         NEXT_ARG();
 719                         if (get_integer(&off, *argv, 0))
 720                                 return -1;
 721                         sel->off = off;
 722                         sel->flags |= TC_U32_OFFSET;
 723                 } else if (matches(*argv, "at") == 0) {
 724                         int off;
 725                         NEXT_ARG();
 726                         if (get_integer(&off, *argv, 0))
 727                                 return -1;
 728                         sel->offoff = off;
 729                         if (off%2) {
 730                                 fprintf(stderr, "offset \"at\" must be even\n");
 731                                 return -1;
 732                         }
 733                         sel->flags |= TC_U32_VAROFFSET;
 734                 } else if (matches(*argv, "mask") == 0) {
 735                         __u16 mask;
 736                         NEXT_ARG();
 737                         if (get_u16(&mask, *argv, 16))
 738                                 return -1;
 739                         sel->offmask = htons(mask);
 740                         sel->flags |= TC_U32_VAROFFSET;
 741                 } else if (matches(*argv, "shift") == 0) {
 742                         int shift;
 743                         NEXT_ARG();
 744                         if (get_integer(&shift, *argv, 0))
 745                                 return -1;
 746                         sel->offshift = shift;
 747                         sel->flags |= TC_U32_VAROFFSET;
 748                 } else if (matches(*argv, "eat") == 0) {
 749                         sel->flags |= TC_U32_EAT;
 750                 } else {
 751                         break;
 752                 }
 753                 argc--; argv++;
 754         }
 755
 756         *argc_p = argc;
 757         *argv_p = argv;
 758         return 0;
 759 }
 760
 761 static int parse_hashkey(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
 762 {
 763         int argc = *argc_p;
 764         char **argv = *argv_p;
 765
 766         while (argc > 0) {
 767                 if (matches(*argv, "mask") == 0) {
 768                         __u32 mask;
 769                         NEXT_ARG();
 770                         if (get_u32(&mask, *argv, 16))
 771                                 return -1;
 772                         sel->hmask = htonl(mask);
 773                 } else if (matches(*argv, "at") == 0) {
 774                         int num;
 775                         NEXT_ARG();
 776                         if (get_integer(&num, *argv, 0))
 777                                 return -1;
 778                         if (num%4)
 779                                 return -1;
 780                         sel->hoff = num;
 781                 } else {
 782                         break;
 783                 }
 784                 argc--; argv++;
 785         }
 786
 787         *argc_p = argc;
 788         *argv_p = argv;
 789         return 0;
 790 }
 791
 792 static void print_ipv4(FILE *f, const struct tc_u32_key *key)
 793 {
 794         char abuf[256];
 795
 796         switch (key->off) {
 797         case 0:
 798                 switch (ntohl(key->mask)) {
 799                 case 0x0f000000:
 800                         fprintf(f, "\n  match IP ihl %u", ntohl(key->val) >> 24);
 801                         return;
 802                 case 0x00ff0000:
 803                         fprintf(f, "\n  match IP dsfield %#x", ntohl(key->val) >> 16);
 804                         return;
 805                 }
 806                 break;
 807         case 8:
 808                 if (ntohl(key->mask) == 0x00ff0000) {
 809                         fprintf(f, "\n  match IP protocol %d", ntohl(key->val) >> 16);
 810                         return;
 811                 }
 812                 break;
 813         case 12:
 814         case 16: {
 815                         int bits = mask2bits(key->mask);
 816                         if (bits >= 0) {
 817                                 fprintf(f, "\n  %s %s/%d",
 818                                         key->off == 12 ? "match IP src" : "match IP dst",
 819                                         inet_ntop(AF_INET, &key->val,
 820                                                   abuf, sizeof(abuf)),
 821                                         bits);
 822                                 return;
 823                         }
 824                 }
 825                 break;
 826
 827         case 20:
 828                 switch (ntohl(key->mask)) {
 829                 case 0x0000ffff:
 830                         fprintf(f, "\n  match sport %u",
 831                                 ntohl(key->val) & 0xffff);
 832                         return;
 833                 case 0xffff0000:
 834                         fprintf(f, "\n  match dport %u",
 835                                 ntohl(key->val) >> 16);
 836                         return;
 837                 case 0xffffffff:
 838                         fprintf(f, "\n  match sport %u, match dport %u",
 839                                 ntohl(key->val) & 0xffff,
 840                                 ntohl(key->val) >> 16);
 841
 842                         return;
 843                 }
 844                 /* XXX: Default print_raw */
 845         }
 846 }
 847
 848 static void print_raw(FILE *f, const struct tc_u32_key *key)
 849 {
 850         fprintf(f, "\n  match %08x/%08x at %s%d",
 851                 (unsigned int)ntohl(key->val),
 852                 (unsigned int)ntohl(key->mask),
 853                 key->offmask ? "nexthdr+" : "",
 854                 key->off);
 855 }
 856
 857 static const struct {
 858         __u16 proto;
 859         __u16 pad;
 860         void (*pprinter)(FILE *f, const struct tc_u32_key *key);
 861 } u32_pprinters[] = {
 862         {0,        0, print_raw},
 863         {ETH_P_IP, 0, print_ipv4},
 864 };
 865
 866 static void show_keys(FILE *f, const struct tc_u32_key *key)
 867 {
 868         int i = 0;
 869
 870         if (!show_pretty)
 871                 goto show_k;
 872
 873         for (i = 0; i < sizeof(u32_pprinters) / sizeof(u32_pprinters[0]); i++) {
 874                 if (u32_pprinters[i].proto == ntohs(f_proto)) {
 875 show_k:
 876                         u32_pprinters[i].pprinter(f, key);
 877                         return;
 878                 }
 879         }
 880
 881         i = 0;
 882         goto show_k;
 883 }
 884
 885 static int u32_parse_opt(struct filter_util *qu, char *handle,
 886                          int argc, char **argv, struct nlmsghdr *n)
 887 {
 888         struct {
 889                 struct tc_u32_sel sel;
 890                 struct tc_u32_key keys[128];
 891         } sel;
 892         struct tcmsg *t = NLMSG_DATA(n);
 893         struct rtattr *tail;
 894         int sel_ok = 0, terminal_ok = 0;
 895         int sample_ok = 0;
 896         __u32 htid = 0;
 897         __u32 order = 0;
 898
 899         memset(&sel, 0, sizeof(sel));
 900
 901         if (handle && get_u32_handle(&t->tcm_handle, handle)) {
 902                 fprintf(stderr, "Illegal filter ID\n");
 903                 return -1;
 904         }
 905
 906         if (argc == 0)
 907                 return 0;
 908
 909         tail = NLMSG_TAIL(n);
 910         addattr_l(n, MAX_MSG, TCA_OPTIONS, NULL, 0);
 911
 912         while (argc > 0) {
 913                 if (matches(*argv, "match") == 0) {
 914                         NEXT_ARG();
 915                         if (parse_selector(&argc, &argv, &sel.sel, n)) {
 916                                 fprintf(stderr, "Illegal \"match\"\n");
 917                                 return -1;
 918                         }
 919                         sel_ok++;
 920                         continue;
 921                 } else if (matches(*argv, "offset") == 0) {
 922                         NEXT_ARG();
 923                         if (parse_offset(&argc, &argv, &sel.sel)) {
 924                                 fprintf(stderr, "Illegal \"offset\"\n");
 925                                 return -1;
 926                         }
 927                         continue;
 928                 } else if (matches(*argv, "hashkey") == 0) {
 929                         NEXT_ARG();
 930                         if (parse_hashkey(&argc, &argv, &sel.sel)) {
 931                                 fprintf(stderr, "Illegal \"hashkey\"\n");
 932                                 return -1;
 933                         }
 934                         continue;
 935                 } else if (matches(*argv, "classid") == 0 ||
 936                            strcmp(*argv, "flowid") == 0) {
 937                         unsigned handle;
 938                         NEXT_ARG();
 939                         if (get_tc_classid(&handle, *argv)) {
 940                                 fprintf(stderr, "Illegal \"classid\"\n");
 941                                 return -1;
 942                         }
 943                         addattr_l(n, MAX_MSG, TCA_U32_CLASSID, &handle, 4);
 944                         sel.sel.flags |= TC_U32_TERMINAL;
 945                 } else if (matches(*argv, "divisor") == 0) {
 946                         unsigned divisor;
 947                         NEXT_ARG();
 948                         if (get_unsigned(&divisor, *argv, 0) ||
 949                             divisor == 0 ||
 950                             divisor > 0x100 || ((divisor - 1) & divisor)) {
 951                                 fprintf(stderr, "Illegal \"divisor\"\n");
 952                                 return -1;
 953                         }
 954                         addattr_l(n, MAX_MSG, TCA_U32_DIVISOR, &divisor, 4);
 955                 } else if (matches(*argv, "order") == 0) {
 956                         NEXT_ARG();
 957                         if (get_u32(&order, *argv, 0)) {
 958                                 fprintf(stderr, "Illegal \"order\"\n");
 959                                 return -1;
 960                         }
 961                 } else if (strcmp(*argv, "link") == 0) {
 962                         unsigned handle;
 963                         NEXT_ARG();
 964                         if (get_u32_handle(&handle, *argv)) {
 965                                 fprintf(stderr, "Illegal \"link\"\n");
 966                                 return -1;
 967                         }
 968                         if (handle && TC_U32_NODE(handle)) {
 969                                 fprintf(stderr, "\"link\" must be a hash table.\n");
 970                                 return -1;
 971                         }
 972                         addattr_l(n, MAX_MSG, TCA_U32_LINK, &handle, 4);
 973                 } else if (strcmp(*argv, "ht") == 0) {
 974                         unsigned handle;
 975                         NEXT_ARG();
 976                         if (get_u32_handle(&handle, *argv)) {
 977                                 fprintf(stderr, "Illegal \"ht\"\n");
 978                                 return -1;
 979                         }
 980                         if (handle && TC_U32_NODE(handle)) {
 981                                 fprintf(stderr, "\"ht\" must be a hash table.\n");
 982                                 return -1;
 983                         }
 984                         if (sample_ok)
 985                                 htid = (htid&0xFF000)|(handle&0xFFF00000);
 986                         else
 987                                 htid = (handle&0xFFFFF000);
 988                 } else if (strcmp(*argv, "sample") == 0) {
 989                         __u32 hash;
 990                         unsigned divisor = 0x100;
 991
 992                         struct {
 993                                 struct tc_u32_sel sel;
 994                                 struct tc_u32_key keys[4];
 995                         } sel2;
 996                         memset(&sel2, 0, sizeof(sel2));
 997                         NEXT_ARG();
 998                         if (parse_selector(&argc, &argv, &sel2.sel, n)) {
 999                                 fprintf(stderr, "Illegal \"sample\"\n");
1000                                 return -1;
1001                         }
1002                         if (sel2.sel.nkeys != 1) {
1003                                 fprintf(stderr, "\"sample\" must contain"
1004                                         " exactly ONE key.\n");
1005                                 return -1;
1006                         }
1007                         if (*argv != 0 && strcmp(*argv, "divisor") == 0) {
1008                                 NEXT_ARG();
1009                                 if (get_unsigned(&divisor, *argv, 0) || divisor == 0 ||
1010                                     divisor > 0x100 || ((divisor - 1) & divisor)) {
1011                                         fprintf(stderr, "Illegal sample \"divisor\"\n");
1012                                         return -1;
1013                                 }
1014                                 NEXT_ARG();
1015                         }
1016                         hash = sel2.sel.keys[0].val&sel2.sel.keys[0].mask;
1017                         hash ^= hash>>16;
1018                         hash ^= hash>>8;
1019                         htid = ((hash%divisor)<<12)|(htid&0xFFF00000);
1020                         sample_ok = 1;
1021                         continue;
1022                 } else if (strcmp(*argv, "indev") == 0) {
1023                         char ind[IFNAMSIZ + 1];
1024                         memset(ind, 0, sizeof (ind));
1025                         argc--;
1026                         argv++;
1027                         if (argc < 1) {
1028                                 fprintf(stderr, "Illegal indev\n");
1029                                 return -1;
1030                         }
1031                         strncpy(ind, *argv, sizeof (ind) - 1);
1032                         addattr_l(n, MAX_MSG, TCA_U32_INDEV, ind, strlen(ind) + 1);
1033
1034                 } else if (matches(*argv, "action") == 0) {
1035                         NEXT_ARG();
1036                         if (parse_action(&argc, &argv, TCA_U32_ACT, n)) {
1037                                 fprintf(stderr, "Illegal \"action\"\n");
1038                                 return -1;
1039                         }
1040                         terminal_ok++;
1041                         continue;
1042
1043                 } else if (matches(*argv, "police") == 0) {
1044                         NEXT_ARG();
1045                         if (parse_police(&argc, &argv, TCA_U32_POLICE, n)) {
1046                                 fprintf(stderr, "Illegal \"police\"\n");
1047                                 return -1;
1048                         }
1049                         terminal_ok++;
1050                         continue;
1051                 } else if (strcmp(*argv, "help") == 0) {
1052                         explain();
1053                         return -1;
1054                 } else {
1055                         fprintf(stderr, "What is \"%s\"?\n", *argv);
1056                         explain();
1057                         return -1;
1058                 }
1059                 argc--; argv++;
1060         }
1061
1062         /* We dont necessarily need class/flowids */
1063         if (terminal_ok)
1064                 sel.sel.flags |= TC_U32_TERMINAL;
1065
1066         if (order) {
1067                 if (TC_U32_NODE(t->tcm_handle) && order != TC_U32_NODE(t->tcm_handle)) {
1068                         fprintf(stderr, "\"order\" contradicts \"handle\"\n");
1069                         return -1;
1070                 }
1071                 t->tcm_handle |= order;
1072         }
1073
1074         if (htid)
1075                 addattr_l(n, MAX_MSG, TCA_U32_HASH, &htid, 4);
1076         if (sel_ok)
1077                 addattr_l(n, MAX_MSG, TCA_U32_SEL, &sel,
1078                           sizeof(sel.sel)+sel.sel.nkeys*sizeof(struct tc_u32_key));
1079         tail->rta_len = (void *) NLMSG_TAIL(n) - (void *) tail;
1080         return 0;
1081 }
1082
1083 static int u32_print_opt(struct filter_util *qu, FILE *f, struct rtattr *opt,
1084                          __u32 handle)
1085 {
1086         struct rtattr *tb[TCA_U32_MAX+1];
1087         struct tc_u32_sel *sel = NULL;
1088         struct tc_u32_pcnt *pf = NULL;
1089
1090         if (opt == NULL)
1091                 return 0;
1092
1093         parse_rtattr_nested(tb, TCA_U32_MAX, opt);
1094
1095         if (handle) {
1096                 SPRINT_BUF(b1);
1097                 fprintf(f, "fh %s ", sprint_u32_handle(handle, b1));
1098         }
1099         if (TC_U32_NODE(handle)) {
1100                 fprintf(f, "order %d ", TC_U32_NODE(handle));
1101         }
1102
1103         if (tb[TCA_U32_SEL]) {
1104                 if (RTA_PAYLOAD(tb[TCA_U32_SEL])  < sizeof(*sel))
1105                         return -1;
1106
1107                 sel = RTA_DATA(tb[TCA_U32_SEL]);
1108         }
1109
1110         if (tb[TCA_U32_DIVISOR]) {
1111                 fprintf(f, "ht divisor %d ", *(__u32*)RTA_DATA(tb[TCA_U32_DIVISOR]));
1112         } else if (tb[TCA_U32_HASH]) {
1113                 __u32 htid = *(__u32*)RTA_DATA(tb[TCA_U32_HASH]);
1114                 fprintf(f, "key ht %x bkt %x ", TC_U32_USERHTID(htid),
1115                         TC_U32_HASH(htid));
1116         } else {
1117                 fprintf(f, "??? ");
1118         }
1119         if (tb[TCA_U32_CLASSID]) {
1120                 SPRINT_BUF(b1);
1121                 fprintf(f, "%sflowid %s ",
1122                         !sel || !(sel->flags&TC_U32_TERMINAL) ? "*" : "",
1123                         sprint_tc_classid(*(__u32*)RTA_DATA(tb[TCA_U32_CLASSID]), b1));
1124         } else if (sel && sel->flags&TC_U32_TERMINAL) {
1125                 fprintf(f, "terminal flowid ??? ");
1126         }
1127         if (tb[TCA_U32_LINK]) {
1128                 SPRINT_BUF(b1);
1129                 fprintf(f, "link %s ",
1130                         sprint_u32_handle(*(__u32*)RTA_DATA(tb[TCA_U32_LINK]), b1));
1131         }
1132
1133         if (tb[TCA_U32_PCNT]) {
1134                 if (RTA_PAYLOAD(tb[TCA_U32_PCNT])  < sizeof(*pf)) {
1135                         fprintf(f, "Broken perf counters \n");
1136                         return -1;
1137                 }
1138                 pf = RTA_DATA(tb[TCA_U32_PCNT]);
1139         }
1140
1141         if (sel && show_stats && NULL != pf)
1142                 fprintf(f, " (rule hit %llu success %llu)",
1143                         (unsigned long long) pf->rcnt,
1144                         (unsigned long long) pf->rhit);
1145
1146         if (tb[TCA_U32_MARK]) {
1147                 struct tc_u32_mark *mark = RTA_DATA(tb[TCA_U32_MARK]);
1148                 if (RTA_PAYLOAD(tb[TCA_U32_MARK]) < sizeof(*mark)) {
1149                         fprintf(f, "\n  Invalid mark (kernel&iproute2 mismatch)\n");
1150                 } else {
1151                         fprintf(f, "\n  mark 0x%04x 0x%04x (success %d)",
1152                                 mark->val, mark->mask, mark->success);
1153                 }
1154         }
1155
1156         if (sel) {
1157                 if (sel->nkeys) {
1158                         int i;
1159                         for (i=0; i<sel->nkeys; i++) {
1160                                 show_keys(f, sel->keys + i);
1161                                 if (show_stats && NULL != pf)
1162                                         fprintf(f, " (success %llu ) ",
1163                                                 (unsigned long long) pf->kcnts[i]);
1164                         }
1165                 }
1166
1167                 if (sel->flags&(TC_U32_VAROFFSET|TC_U32_OFFSET)) {
1168                         fprintf(f, "\n    offset ");
1169                         if (sel->flags&TC_U32_VAROFFSET)
1170                                 fprintf(f, "%04x>>%d at %d ",
1171                                         ntohs(sel->offmask),
1172                                         sel->offshift,  sel->offoff);
1173                         if (sel->off)
1174                                 fprintf(f, "plus %d ", sel->off);
1175                 }
1176                 if (sel->flags&TC_U32_EAT)
1177                         fprintf(f, " eat ");
1178
1179                 if (sel->hmask) {
1180                         fprintf(f, "\n    hash mask %08x at %d ",
1181                                 (unsigned int)htonl(sel->hmask), sel->hoff);
1182                 }
1183         }
1184
1185         if (tb[TCA_U32_POLICE]) {
1186                 fprintf(f, "\n");
1187                 tc_print_police(f, tb[TCA_U32_POLICE]);
1188         }
1189         if (tb[TCA_U32_INDEV]) {
1190                 struct rtattr *idev = tb[TCA_U32_INDEV];
1191                 fprintf(f, "\n  input dev %s\n", (char *) RTA_DATA(idev));
1192         }
1193         if (tb[TCA_U32_ACT]) {
1194                 tc_print_action(f, tb[TCA_U32_ACT]);
1195         }
1196
1197         return 0;
1198 }
1199
1200 struct filter_util u32_filter_util = {
1201         .id = "u32",
1202         .parse_fopt = u32_parse_opt,
1203         .print_fopt = u32_print_opt,
1204 };