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