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