tc/m_pedit.c

   1 /*
   2  * m_pedit.c            generic packet editor actions module
   3  *
   4  *              This program is free software; you can distribute 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:  J Hadi Salim (hadi@cyberus.ca)
  10  *
  11  * TODO:
  12  *      1) Big endian broken in some spots
  13  *      2) A lot of this stuff was added on the fly; get a big double-double
  14  *      and clean it up at some point.
  15  *
  16  */
  17
  18 #include <stdio.h>
  19 #include <stdlib.h>
  20 #include <unistd.h>
  21 #include <syslog.h>
  22 #include <fcntl.h>
  23 #include <sys/socket.h>
  24 #include <netinet/in.h>
  25 #include <arpa/inet.h>
  26 #include <string.h>
  27 #include <dlfcn.h>
  28 #include "utils.h"
  29 #include "tc_util.h"
  30 #include "m_pedit.h"
  31
  32 static struct m_pedit_util *pedit_list;
  33 int pedit_debug = 1;
  34
  35 static void
  36 explain(void)
  37 {
  38         fprintf(stderr, "Usage: ... pedit munge <MUNGE>\n");
  39         fprintf(stderr,
  40                 "Where: MUNGE := <RAW>|<LAYERED>\n"
  41                 "\t<RAW>:= <OFFSETC>[ATC]<CMD>\n "
  42                 "\t\tOFFSETC:= offset <offval> <u8|u16|u32>\n "
  43                 "\t\tATC:= at <atval> offmask <maskval> shift <shiftval>\n "
  44                 "\t\tNOTE: offval is byte offset, must be multiple of 4\n "
  45                 "\t\tNOTE: maskval is a 32 bit hex number\n "
  46                 "\t\tNOTE: shiftval is a is a shift value\n "
  47                 "\t\tCMD:= clear | invert | set <setval>| retain\n "
  48                 "\t<LAYERED>:= ip <ipdata> | ip6 <ip6data> \n "
  49                 " \t\t| udp <udpdata> | tcp <tcpdata> | icmp <icmpdata> \n"
  50                 "For Example usage look at the examples directory\n");
  51
  52 }
  53
  54 static void
  55 usage(void)
  56 {
  57         explain();
  58         exit(-1);
  59 }
  60
  61 static int
  62 pedit_parse_nopopt (int *argc_p, char ***argv_p,struct tc_pedit_sel *sel,struct tc_pedit_key *tkey)
  63 {
  64         int argc = *argc_p;
  65         char **argv = *argv_p;
  66
  67         if (argc) {
  68                 fprintf(stderr, "Unknown action  hence option \"%s\" is unparsable\n", *argv);
  69                         return -1;
  70         }
  71
  72         return 0;
  73
  74 }
  75
  76 struct m_pedit_util
  77 *get_pedit_kind(char *str)
  78 {
  79         static void *pBODY;
  80         void *dlh;
  81         char buf[256];
  82         struct  m_pedit_util *p;
  83
  84         for (p = pedit_list; p; p = p->next) {
  85                 if (strcmp(p->id, str) == 0)
  86                         return p;
  87         }
  88
  89         snprintf(buf, sizeof(buf), "p_%s.so", str);
  90         dlh = dlopen(buf, RTLD_LAZY);
  91         if (dlh == NULL) {
  92                 dlh = pBODY;
  93                 if (dlh == NULL) {
  94                         dlh = pBODY = dlopen(NULL, RTLD_LAZY);
  95                         if (dlh == NULL)
  96                                 goto noexist;
  97                 }
  98         }
  99
 100         snprintf(buf, sizeof(buf), "p_pedit_%s", str);
 101         p = dlsym(dlh, buf);
 102         if (p == NULL)
 103                 goto noexist;
 104
 105 reg:
 106         p->next = pedit_list;
 107         pedit_list = p;
 108         return p;
 109
 110 noexist:
 111         p = malloc(sizeof(*p));
 112         if (p) {
 113                 memset(p, 0, sizeof(*p));
 114                 strncpy(p->id, str, sizeof(p->id)-1);
 115                 p->parse_peopt = pedit_parse_nopopt;
 116                 goto reg;
 117         }
 118         return p;
 119 }
 120
 121 int
 122 pack_key(struct tc_pedit_sel *sel,struct tc_pedit_key *tkey)
 123 {
 124         int hwm = sel->nkeys;
 125
 126         if (hwm >= MAX_OFFS)
 127                 return -1;
 128
 129         if (tkey->off % 4) {
 130                 fprintf(stderr, "offsets MUST be in 32 bit boundaries\n");
 131                 return -1;
 132         }
 133
 134         sel->keys[hwm].val = tkey->val;
 135         sel->keys[hwm].mask = tkey->mask;
 136         sel->keys[hwm].off = tkey->off;
 137         sel->keys[hwm].at = tkey->at;
 138         sel->keys[hwm].offmask = tkey->offmask;
 139         sel->keys[hwm].shift = tkey->shift;
 140         sel->nkeys++;
 141         return 0;
 142 }
 143
 144
 145 int
 146 pack_key32(__u32 retain,struct tc_pedit_sel *sel,struct tc_pedit_key *tkey)
 147 {
 148         if (tkey->off > (tkey->off & ~3)) {
 149                 fprintf(stderr,
 150                         "pack_key32: 32 bit offsets must begin in 32bit boundaries\n");
 151                 return -1;
 152         }
 153
 154         tkey->val = htonl(tkey->val & retain);
 155         tkey->mask = htonl(tkey->mask | ~retain);
 156         /* jamal remove this - it is not necessary given the if check above */
 157         tkey->off &= ~3;
 158         return pack_key(sel,tkey);
 159 }
 160
 161 int
 162 pack_key16(__u32 retain,struct tc_pedit_sel *sel,struct tc_pedit_key *tkey)
 163 {
 164         int ind = 0, stride = 0;
 165         __u32 m[4] = {0xFFFF0000,0xFF0000FF,0x0000FFFF};
 166
 167         if (0 > tkey->off) {
 168                 ind = tkey->off + 1;
 169                 if (0 > ind)
 170                         ind = -1*ind;
 171         } else {
 172                 ind = tkey->off;
 173         }
 174
 175         if (tkey->val > 0xFFFF || tkey->mask > 0xFFFF) {
 176                 fprintf(stderr, "pack_key16 bad value\n");
 177                 return -1;
 178         }
 179
 180         ind = tkey->off & 3;
 181
 182         if (0 > ind || 2 < ind) {
 183                 fprintf(stderr, "pack_key16 bad index value %d\n",ind);
 184                 return -1;
 185         }
 186
 187         stride = 8 * ind;
 188         tkey->val = htons(tkey->val);
 189         if (stride > 0) {
 190                 tkey->val <<= stride;
 191                 tkey->mask <<= stride;
 192                 retain <<= stride;
 193         }
 194         tkey->mask = retain|m[ind];
 195
 196         tkey->off &= ~3;
 197
 198         if (pedit_debug)
 199                 printf("pack_key16: Final val %08x mask %08x \n",tkey->val,tkey->mask);
 200         return pack_key(sel,tkey);
 201
 202 }
 203
 204 int
 205 pack_key8(__u32 retain,struct tc_pedit_sel *sel,struct tc_pedit_key *tkey)
 206 {
 207         int ind = 0, stride = 0;
 208         __u32 m[4] = {0xFFFFFF00,0xFFFF00FF,0xFF00FFFF,0x00FFFFFF};
 209
 210         if (0 > tkey->off) {
 211                 ind = tkey->off + 1;
 212                 if (0 > ind)
 213                         ind = -1*ind;
 214         } else {
 215                 ind = tkey->off;
 216         }
 217
 218         if (tkey->val > 0xFF || tkey->mask > 0xFF) {
 219                 fprintf(stderr, "pack_key8 bad value (val %x mask %x\n", tkey->val, tkey->mask);
 220                 return -1;
 221         }
 222
 223         ind = tkey->off & 3;
 224         stride = 8 * ind;
 225         tkey->val <<= stride;
 226         tkey->mask <<= stride;
 227         retain <<= stride;
 228         tkey->mask = retain|m[ind];
 229         tkey->off &= ~3;
 230
 231         if (pedit_debug)
 232                 printf("pack_key8: Final word off %d  val %08x mask %08x \n",tkey->off , tkey->val,tkey->mask);
 233         return pack_key(sel,tkey);
 234 }
 235
 236 int
 237 parse_val(int *argc_p, char ***argv_p, __u32 * val, int type)
 238 {
 239         int argc = *argc_p;
 240         char **argv = *argv_p;
 241
 242         if (argc <= 0)
 243                 return -1;
 244
 245         if (TINT == type)
 246                 return get_integer((int *) val, *argv, 0);
 247
 248         if (TU32 == type)
 249                 return get_u32(val, *argv, 0);
 250
 251         if (TIPV4 == type) {
 252                 inet_prefix addr;
 253                 if (get_prefix_1(&addr, *argv, AF_INET)) {
 254                         return -1;
 255                 }
 256                 *val=addr.data[0];
 257                 return 0;
 258         }
 259         if (TIPV6 == type) {
 260                 /* not implemented yet */
 261                 return -1;
 262         }
 263
 264         return -1;
 265 }
 266
 267 int
 268 parse_cmd(int *argc_p, char ***argv_p, __u32 len, int type,__u32 retain,struct tc_pedit_sel *sel,struct tc_pedit_key *tkey)
 269 {
 270         __u32 mask = 0, val = 0;
 271         __u32 o = 0xFF;
 272         int res = -1;
 273         int argc = *argc_p;
 274         char **argv = *argv_p;
 275
 276         if (argc <= 0)
 277                 return -1;
 278
 279         if (pedit_debug)
 280                 printf("parse_cmd argc %d %s offset %d length %d\n",argc,*argv,tkey->off,len);
 281
 282         if (len == 2)
 283                 o = 0xFFFF;
 284         if (len == 4)
 285                 o = 0xFFFFFFFF;
 286
 287         if (matches(*argv, "invert") == 0) {
 288                 retain = val = mask = o;
 289         } else if (matches(*argv, "set") == 0) {
 290                 NEXT_ARG();
 291                 if (parse_val(&argc, &argv, &val, type))
 292                         return -1;
 293         } else if (matches(*argv, "preserve") == 0) {
 294                 retain = mask = o;
 295         } else {
 296                 if (matches(*argv, "clear") != 0)
 297                         return -1;
 298         }
 299
 300         argc--; argv++;
 301
 302         if (argc && matches(*argv, "retain") == 0) {
 303                 NEXT_ARG();
 304                 if (parse_val(&argc, &argv, &retain, TU32))
 305                         return -1;
 306                 argc--; argv++;
 307         }
 308
 309         tkey->val = val;
 310
 311         if (len == 1) {
 312                 tkey->mask = 0xFF;
 313                 res = pack_key8(retain,sel,tkey);
 314                 goto done;
 315         }
 316         if (len == 2) {
 317                 tkey->mask = mask;
 318                 res = pack_key16(retain,sel,tkey);
 319                 goto done;
 320         }
 321         if (len == 4) {
 322                 tkey->mask = mask;
 323                 res = pack_key32(retain,sel,tkey);
 324                 goto done;
 325         }
 326
 327         return -1;
 328 done:
 329         if (pedit_debug)
 330                 printf("parse_cmd done argc %d %s offset %d length %d\n",argc,*argv,tkey->off,len);
 331         *argc_p = argc;
 332         *argv_p = argv;
 333         return res;
 334
 335 }
 336
 337 int
 338 parse_offset(int *argc_p, char ***argv_p,struct tc_pedit_sel *sel,struct tc_pedit_key *tkey)
 339 {
 340         int off;
 341         __u32 len, retain;
 342         int argc = *argc_p;
 343         char **argv = *argv_p;
 344         int res = -1;
 345
 346         if (argc <= 0)
 347                 return -1;
 348
 349         if (get_integer(&off, *argv, 0))
 350                 return -1;
 351         tkey->off = off;
 352
 353         argc--;
 354         argv++;
 355
 356         if (argc <= 0)
 357                 return -1;
 358
 359
 360         if (matches(*argv, "u32") == 0) {
 361                 len = 4;
 362                 retain = 0xFFFFFFFF;
 363                 goto done;
 364         }
 365         if (matches(*argv, "u16") == 0) {
 366                 len = 2;
 367                 retain = 0x0;
 368                 goto done;
 369         }
 370         if (matches(*argv, "u8") == 0) {
 371                 len = 1;
 372                 retain = 0x0;
 373                 goto done;
 374         }
 375
 376         return -1;
 377
 378 done:
 379
 380         NEXT_ARG();
 381
 382         /* [at <someval> offmask <maskval> shift <shiftval>] */
 383         if (matches(*argv, "at") == 0) {
 384
 385                 __u32 atv=0,offmask=0x0,shift=0;
 386
 387                 NEXT_ARG();
 388                 if (get_u32(&atv, *argv, 0))
 389                         return -1;
 390                 tkey->at = atv;
 391
 392                 NEXT_ARG();
 393
 394                 if (get_u32(&offmask, *argv, 16))
 395                         return -1;
 396                 tkey->offmask = offmask;
 397
 398                 NEXT_ARG();
 399
 400                 if (get_u32(&shift, *argv, 0))
 401                         return -1;
 402                 tkey->shift = shift;
 403
 404                 NEXT_ARG();
 405         }
 406
 407         res = parse_cmd(&argc, &argv, len, TU32,retain,sel,tkey);
 408
 409         *argc_p = argc;
 410         *argv_p = argv;
 411         return res;
 412 }
 413
 414 int
 415 parse_munge(int *argc_p, char ***argv_p,struct tc_pedit_sel *sel)
 416 {
 417         struct tc_pedit_key tkey;
 418         int argc = *argc_p;
 419         char **argv = *argv_p;
 420         int res = -1;
 421
 422         if (argc <= 0)
 423                 return -1;
 424
 425         memset(&tkey, 0, sizeof(tkey));
 426
 427         if (matches(*argv, "offset") == 0) {
 428                 NEXT_ARG();
 429                 res = parse_offset(&argc, &argv,sel,&tkey);
 430                 goto done;
 431         } else {
 432                 char k[16];
 433                 struct m_pedit_util *p = NULL;
 434
 435                 strncpy(k, *argv, sizeof (k) - 1);
 436
 437                 if (argc > 0 ) {
 438                         p = get_pedit_kind(k);
 439                         if (NULL == p)
 440                                 goto bad_val;
 441                         res = p->parse_peopt(&argc, &argv, sel,&tkey);
 442                         if (res < 0) {
 443                                 fprintf(stderr,"bad pedit parsing\n");
 444                                 goto bad_val;
 445                         }
 446                         goto done;
 447                 }
 448         }
 449
 450 bad_val:
 451         return -1;
 452
 453 done:
 454
 455         *argc_p = argc;
 456         *argv_p = argv;
 457         return res;
 458 }
 459
 460 int
 461 parse_pedit(struct action_util *a, int *argc_p, char ***argv_p, int tca_id, struct nlmsghdr *n)
 462 {
 463         struct {
 464                 struct tc_pedit_sel sel;
 465                 struct tc_pedit_key keys[MAX_OFFS];
 466         } sel;
 467
 468         int argc = *argc_p;
 469         char **argv = *argv_p;
 470         int ok = 0, iok = 0;
 471         struct rtattr *tail;
 472
 473         memset(&sel, 0, sizeof(sel));
 474
 475         while (argc > 0) {
 476                 if (pedit_debug > 1)
 477                         fprintf(stderr, "while pedit (%d:%s)\n",argc, *argv);
 478                 if (matches(*argv, "pedit") == 0) {
 479                         NEXT_ARG();
 480                         ok++;
 481                         continue;
 482                 } else if (matches(*argv, "help") == 0) {
 483                         usage();
 484                 } else if (matches(*argv, "munge") == 0) {
 485                         if (!ok) {
 486                                 fprintf(stderr, "Illegal pedit construct (%s) \n", *argv);
 487                                 explain();
 488                                 return -1;
 489                         }
 490                         NEXT_ARG();
 491                         if (parse_munge(&argc, &argv,&sel.sel)) {
 492                                 fprintf(stderr, "Illegal pedit construct (%s) \n", *argv);
 493                                 explain();
 494                                 return -1;
 495                         }
 496                         ok++;
 497                 } else {
 498                         break;
 499                 }
 500
 501         }
 502
 503         if (!ok) {
 504                 explain();
 505                 return -1;
 506         }
 507
 508         if (argc) {
 509                 if (matches(*argv, "reclassify") == 0) {
 510                         sel.sel.action = TC_ACT_RECLASSIFY;
 511                         NEXT_ARG();
 512                 } else if (matches(*argv, "pipe") == 0) {
 513                         sel.sel.action = TC_ACT_PIPE;
 514                         NEXT_ARG();
 515                 } else if (matches(*argv, "drop") == 0 ||
 516                         matches(*argv, "shot") == 0) {
 517                         sel.sel.action = TC_ACT_SHOT;
 518                         NEXT_ARG();
 519                 } else if (matches(*argv, "continue") == 0) {
 520                         sel.sel.action = TC_ACT_UNSPEC;
 521                         NEXT_ARG();
 522                 } else if (matches(*argv, "pass") == 0) {
 523                         sel.sel.action = TC_ACT_OK;
 524                         NEXT_ARG();
 525                 }
 526         }
 527
 528         if (argc) {
 529                 if (matches(*argv, "index") == 0) {
 530                         NEXT_ARG();
 531                         if (get_u32(&sel.sel.index, *argv, 10)) {
 532                                 fprintf(stderr, "Pedit: Illegal \"index\"\n");
 533                                 return -1;
 534                         }
 535                         argc--;
 536                         argv++;
 537                         iok++;
 538                 }
 539         }
 540
 541         tail = NLMSG_TAIL(n);
 542         addattr_l(n, MAX_MSG, tca_id, NULL, 0);
 543         addattr_l(n, MAX_MSG, TCA_PEDIT_PARMS,&sel, sizeof(sel.sel)+sel.sel.nkeys*sizeof(struct tc_pedit_key));
 544         tail->rta_len = (void *) NLMSG_TAIL(n) - (void *) tail;
 545
 546         *argc_p = argc;
 547         *argv_p = argv;
 548         return 0;
 549 }
 550
 551 int
 552 print_pedit(struct action_util *au,FILE * f, struct rtattr *arg)
 553 {
 554         struct tc_pedit_sel *sel;
 555         struct rtattr *tb[TCA_PEDIT_MAX + 1];
 556         SPRINT_BUF(b1);
 557
 558         if (arg == NULL)
 559                 return -1;
 560
 561         parse_rtattr_nested(tb, TCA_PEDIT_MAX, arg);
 562
 563         if (tb[TCA_PEDIT_PARMS] == NULL) {
 564                 fprintf(f, "[NULL pedit parameters]");
 565                 return -1;
 566         }
 567         sel = RTA_DATA(tb[TCA_PEDIT_PARMS]);
 568
 569         fprintf(f, " pedit action %s keys %d\n ", action_n2a(sel->action, b1, sizeof (b1)),sel->nkeys);
 570         fprintf(f, "\t index %d ref %d bind %d", sel->index,sel->refcnt, sel->bindcnt);
 571
 572         if (show_stats) {
 573                 if (tb[TCA_PEDIT_TM]) {
 574                         struct tcf_t *tm = RTA_DATA(tb[TCA_PEDIT_TM]);
 575                         print_tm(f,tm);
 576                 }
 577         }
 578         if (sel->nkeys) {
 579                 int i;
 580                 struct tc_pedit_key *key = sel->keys;
 581
 582                 for (i=0; i<sel->nkeys; i++, key++) {
 583                         fprintf(f, "\n\t key #%d",i);
 584                         fprintf(f, "  at %d: val %08x mask %08x",
 585                         (unsigned int)key->off,
 586                         (unsigned int)ntohl(key->val),
 587                         (unsigned int)ntohl(key->mask));
 588                 }
 589         } else {
 590                 fprintf(f, "\npedit %x keys %d is not LEGIT", sel->index,sel->nkeys);
 591         }
 592
 593
 594         fprintf(f, "\n ");
 595         return 0;
 596 }
 597
 598 int
 599 pedit_print_xstats(struct action_util *au, FILE *f, struct rtattr *xstats)
 600 {
 601         return 0;
 602 }
 603
 604 struct action_util pedit_action_util = {
 605         .id = "pedit",
 606         .parse_aopt = parse_pedit,
 607         .print_aopt = print_pedit,
 608 };