tc/m_ematch.c

   1 /*
   2  * m_ematch.c           Extended Matches
   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:     Thomas Graf <tgraf@suug.ch>
  10  */
  11
  12 #include <stdio.h>
  13 #include <stdlib.h>
  14 #include <unistd.h>
  15 #include <syslog.h>
  16 #include <fcntl.h>
  17 #include <sys/socket.h>
  18 #include <netinet/in.h>
  19 #include <arpa/inet.h>
  20 #include <string.h>
  21 #include <dlfcn.h>
  22 #include <stdarg.h>
  23 #include <errno.h>
  24
  25 #include "utils.h"
  26 #include "tc_util.h"
  27 #include "m_ematch.h"
  28
  29 #define EMATCH_MAP "/etc/iproute2/ematch_map"
  30
  31 static struct ematch_util *ematch_list;
  32
  33 /* export to bison parser */
  34 int ematch_argc;
  35 char **ematch_argv;
  36 char *ematch_err = NULL;
  37 struct ematch *ematch_root;
  38
  39 static int begin_argc;
  40 static char **begin_argv;
  41
  42 static inline void map_warning(int num, char *kind)
  43 {
  44         fprintf(stderr,
  45             "Error: Unable to find ematch \"%s\" in %s\n" \
  46             "Please assign a unique ID to the ematch kind the suggested " \
  47             "entry is:\n" \
  48             "\t%d\t%s\n",
  49             kind, EMATCH_MAP, num, kind);
  50 }
  51
  52 static int lookup_map(__u16 num, char *dst, int len, const char *file)
  53 {
  54         int err = -EINVAL;
  55         char buf[512];
  56         FILE *fd = fopen(file, "r");
  57
  58         if (fd == NULL)
  59                 return -errno;
  60
  61         while (fgets(buf, sizeof(buf), fd)) {
  62                 char namebuf[512], *p = buf;
  63                 int id;
  64
  65                 while (*p == ' ' || *p == '\t')
  66                         p++;
  67                 if (*p == '#' || *p == '\n' || *p == 0)
  68                         continue;
  69
  70                 if (sscanf(p, "%d %s", &id, namebuf) != 2) {
  71                         fprintf(stderr, "ematch map %s corrupted at %s\n",
  72                             file, p);
  73                         goto out;
  74                 }
  75
  76                 if (id == num) {
  77                         if (dst)
  78                                 strncpy(dst, namebuf, len - 1);
  79                         err = 0;
  80                         goto out;
  81                 }
  82         }
  83
  84         err = -ENOENT;
  85 out:
  86         fclose(fd);
  87         return err;
  88 }
  89
  90 static int lookup_map_id(char *kind, int *dst, const char *file)
  91 {
  92         int err = -EINVAL;
  93         char buf[512];
  94         FILE *fd = fopen(file, "r");
  95
  96         if (fd == NULL)
  97                 return -errno;
  98
  99         while (fgets(buf, sizeof(buf), fd)) {
 100                 char namebuf[512], *p = buf;
 101                 int id;
 102
 103                 while (*p == ' ' || *p == '\t')
 104                         p++;
 105                 if (*p == '#' || *p == '\n' || *p == 0)
 106                         continue;
 107
 108                 if (sscanf(p, "%d %s", &id, namebuf) != 2) {
 109                         fprintf(stderr, "ematch map %s corrupted at %s\n",
 110                             file, p);
 111                         goto out;
 112                 }
 113
 114                 if (!strcasecmp(namebuf, kind)) {
 115                         if (dst)
 116                                 *dst = id;
 117                         err = 0;
 118                         goto out;
 119                 }
 120         }
 121
 122         err = -ENOENT;
 123         *dst = 0;
 124 out:
 125         fclose(fd);
 126         return err;
 127 }
 128
 129 static struct ematch_util *get_ematch_kind(char *kind)
 130 {
 131         static void *body;
 132         void *dlh;
 133         char buf[256];
 134         struct ematch_util *e;
 135
 136         for (e = ematch_list; e; e = e->next) {
 137                 if (strcmp(e->kind, kind) == 0)
 138                         return e;
 139         }
 140
 141         snprintf(buf, sizeof(buf), "em_%s.so", kind);
 142         dlh = dlopen(buf, RTLD_LAZY);
 143         if (dlh == NULL) {
 144                 dlh = body;
 145                 if (dlh == NULL) {
 146                         dlh = body = dlopen(NULL, RTLD_LAZY);
 147                         if (dlh == NULL)
 148                                 return NULL;
 149                 }
 150         }
 151
 152         snprintf(buf, sizeof(buf), "%s_ematch_util", kind);
 153         e = dlsym(dlh, buf);
 154         if (e == NULL)
 155                 return NULL;
 156
 157         e->next = ematch_list;
 158         ematch_list = e;
 159
 160         return e;
 161 }
 162
 163 static struct ematch_util *get_ematch_kind_num(__u16 kind)
 164 {
 165         char name[32];
 166
 167         if (lookup_map(kind, name, sizeof(name), EMATCH_MAP) < 0)
 168                 return NULL;
 169
 170         return get_ematch_kind(name);
 171 }
 172
 173 static int parse_tree(struct nlmsghdr *n, struct ematch *tree)
 174 {
 175         int index = 1;
 176         struct ematch *t;
 177
 178         for (t = tree; t; t = t->next) {
 179                 struct rtattr *tail = NLMSG_TAIL(n);
 180                 struct tcf_ematch_hdr hdr = {
 181                         .flags = t->relation
 182                 };
 183
 184                 if (t->inverted)
 185                         hdr.flags |= TCF_EM_INVERT;
 186
 187                 addattr_l(n, MAX_MSG, index++, NULL, 0);
 188
 189                 if (t->child) {
 190                         __u32 r = t->child_ref;
 191                         addraw_l(n, MAX_MSG, &hdr, sizeof(hdr));
 192                         addraw_l(n, MAX_MSG, &r, sizeof(r));
 193                 } else {
 194                         int num = 0, err;
 195                         char buf[64];
 196                         struct ematch_util *e;
 197
 198                         if (t->args == NULL)
 199                                 return -1;
 200
 201                         strncpy(buf, (char*) t->args->data, sizeof(buf)-1);
 202                         e = get_ematch_kind(buf);
 203                         if (e == NULL) {
 204                                 fprintf(stderr, "Unknown ematch \"%s\"\n",
 205                                     buf);
 206                                 return -1;
 207                         }
 208
 209                         err = lookup_map_id(buf, &num, EMATCH_MAP);
 210                         if (err < 0) {
 211                                 if (err == -ENOENT)
 212                                         map_warning(e->kind_num, buf);
 213                                 return err;
 214                         }
 215
 216                         hdr.kind = num;
 217                         if (e->parse_eopt(n, &hdr, t->args->next) < 0)
 218                                 return -1;
 219                 }
 220
 221                 tail->rta_len = (void*) NLMSG_TAIL(n) - (void*) tail;
 222         }
 223
 224         return 0;
 225 }
 226
 227 static int flatten_tree(struct ematch *head, struct ematch *tree)
 228 {
 229         int i, count = 0;
 230         struct ematch *t;
 231
 232         for (;;) {
 233                 count++;
 234
 235                 if (tree->child) {
 236                         for (t = head; t->next; t = t->next);
 237                         t->next = tree->child;
 238                         count += flatten_tree(head, tree->child);
 239                 }
 240
 241                 if (tree->relation == 0)
 242                         break;
 243
 244                 tree = tree->next;
 245         }
 246
 247         for (i = 0, t = head; t; t = t->next, i++)
 248                 t->index = i;
 249
 250         for (t = head; t; t = t->next)
 251                 if (t->child)
 252                         t->child_ref = t->child->index;
 253
 254         return count;
 255 }
 256
 257 int em_parse_error(int err, struct bstr *args, struct bstr *carg,
 258                    struct ematch_util *e, char *fmt, ...)
 259 {
 260         va_list a;
 261
 262         va_start(a, fmt);
 263         vfprintf(stderr, fmt, a);
 264         va_end(a);
 265
 266         if (ematch_err)
 267                 fprintf(stderr, ": %s\n... ", ematch_err);
 268         else
 269                 fprintf(stderr, "\n... ");
 270
 271         while (ematch_argc < begin_argc) {
 272                 if (ematch_argc == (begin_argc - 1))
 273                         fprintf(stderr, ">>%s<< ", *begin_argv);
 274                 else
 275                         fprintf(stderr, "%s ", *begin_argv);
 276                 begin_argv++;
 277                 begin_argc--;
 278         }
 279
 280         fprintf(stderr, "...\n");
 281
 282         if (args) {
 283                 fprintf(stderr, "... %s(", e->kind);
 284                 while (args) {
 285                         fprintf(stderr, "%s", args == carg ? ">>" : "");
 286                         bstr_print(stderr, args, 1);
 287                         fprintf(stderr, "%s%s", args == carg ? "<<" : "",
 288                             args->next ? " " : "");
 289                         args = args->next;
 290                 }
 291                 fprintf(stderr, ")...\n");
 292
 293         }
 294
 295         if (e == NULL) {
 296                 fprintf(stderr,
 297                     "Usage: EXPR\n" \
 298                     "where: EXPR  := TERM [ { and | or } EXPR ]\n" \
 299                     "       TERM  := [ not ] { MATCH | '(' EXPR ')' }\n" \
 300                     "       MATCH := module '(' ARGS ')'\n" \
 301                     "       ARGS := ARG1 ARG2 ...\n" \
 302                     "\n" \
 303                     "Example: a(x y) and not (b(x) or c(x y z))\n");
 304         } else
 305                 e->print_usage(stderr);
 306
 307         return -err;
 308 }
 309
 310 static inline void free_ematch_err(void)
 311 {
 312         if (ematch_err) {
 313                 free(ematch_err);
 314                 ematch_err = NULL;
 315         }
 316 }
 317
 318 extern int ematch_parse(void);
 319
 320 int parse_ematch(int *argc_p, char ***argv_p, int tca_id, struct nlmsghdr *n)
 321 {
 322         begin_argc = ematch_argc = *argc_p;
 323         begin_argv = ematch_argv = *argv_p;
 324
 325         if (ematch_parse()) {
 326                 int err = em_parse_error(EINVAL, NULL, NULL, NULL,
 327                     "Parse error");
 328                 free_ematch_err();
 329                 return err;
 330         }
 331
 332         free_ematch_err();
 333
 334         /* undo look ahead by parser */
 335         ematch_argc++;
 336         ematch_argv--;
 337
 338         if (ematch_root) {
 339                 struct rtattr *tail, *tail_list;
 340
 341                 struct tcf_ematch_tree_hdr hdr = {
 342                         .nmatches = flatten_tree(ematch_root, ematch_root),
 343                         .progid = TCF_EM_PROG_TC
 344                 };
 345
 346                 tail = NLMSG_TAIL(n);
 347                 addattr_l(n, MAX_MSG, tca_id, NULL, 0);
 348                 addattr_l(n, MAX_MSG, TCA_EMATCH_TREE_HDR, &hdr, sizeof(hdr));
 349
 350                 tail_list = NLMSG_TAIL(n);
 351                 addattr_l(n, MAX_MSG, TCA_EMATCH_TREE_LIST, NULL, 0);
 352
 353                 if (parse_tree(n, ematch_root) < 0)
 354                         return -1;
 355
 356                 tail_list->rta_len = (void*) NLMSG_TAIL(n) - (void*) tail_list;
 357                 tail->rta_len = (void*) NLMSG_TAIL(n) - (void*) tail;
 358         }
 359
 360         *argc_p = ematch_argc;
 361         *argv_p = ematch_argv;
 362
 363         return 0;
 364 }
 365
 366 static int print_ematch_seq(FILE *fd, struct rtattr **tb, int start,
 367                             int prefix)
 368 {
 369         int n, i = start;
 370         struct tcf_ematch_hdr *hdr;
 371         int dlen;
 372         void *data;
 373
 374         for (;;) {
 375                 if (tb[i] == NULL)
 376                         return -1;
 377
 378                 dlen = RTA_PAYLOAD(tb[i]) - sizeof(*hdr);
 379                 data = (void *) RTA_DATA(tb[i]) + sizeof(*hdr);
 380
 381                 if (dlen < 0)
 382                         return -1;
 383
 384                 hdr = RTA_DATA(tb[i]);
 385
 386                 if (hdr->flags & TCF_EM_INVERT)
 387                         fprintf(fd, "NOT ");
 388
 389                 if (hdr->kind == 0) {
 390                         __u32 ref;
 391
 392                         if (dlen < sizeof(__u32))
 393                                 return -1;
 394
 395                         ref = *(__u32 *) data;
 396                         fprintf(fd, "(\n");
 397                         for (n = 0; n <= prefix; n++)
 398                                 fprintf(fd, "  ");
 399                         if (print_ematch_seq(fd, tb, ref + 1, prefix + 1) < 0)
 400                                 return -1;
 401                         for (n = 0; n < prefix; n++)
 402                                 fprintf(fd, "  ");
 403                         fprintf(fd, ") ");
 404
 405                 } else {
 406                         struct ematch_util *e;
 407
 408                         e = get_ematch_kind_num(hdr->kind);
 409                         if (e == NULL)
 410                                 fprintf(fd, "[unknown ematch %d]\n",
 411                                     hdr->kind);
 412                         else {
 413                                 fprintf(fd, "%s(", e->kind);
 414                                 if (e->print_eopt(fd, hdr, data, dlen) < 0)
 415                                         return -1;
 416                                 fprintf(fd, ")\n");
 417                         }
 418                         if (hdr->flags & TCF_EM_REL_MASK)
 419                                 for (n = 0; n < prefix; n++)
 420                                         fprintf(fd, "  ");
 421                 }
 422
 423                 switch (hdr->flags & TCF_EM_REL_MASK) {
 424                         case TCF_EM_REL_AND:
 425                                 fprintf(fd, "AND ");
 426                                 break;
 427
 428                         case TCF_EM_REL_OR:
 429                                 fprintf(fd, "OR ");
 430                                 break;
 431
 432                         default:
 433                                 return 0;
 434                 }
 435
 436                 i++;
 437         }
 438
 439         return 0;
 440 }
 441
 442 static int print_ematch_list(FILE *fd, struct tcf_ematch_tree_hdr *hdr,
 443                              struct rtattr *rta)
 444 {
 445         int err = -1;
 446         struct rtattr **tb;
 447
 448         tb = malloc((hdr->nmatches + 1) * sizeof(struct rtattr *));
 449         if (tb == NULL)
 450                 return -1;
 451
 452         if (hdr->nmatches > 0) {
 453                 if (parse_rtattr_nested(tb, hdr->nmatches, rta) < 0)
 454                         goto errout;
 455
 456                 fprintf(fd, "\n  ");
 457                 if (print_ematch_seq(fd, tb, 1, 1) < 0)
 458                         goto errout;
 459         }
 460
 461         err = 0;
 462 errout:
 463         free(tb);
 464         return err;
 465 }
 466
 467 int print_ematch(FILE *fd, const struct rtattr *rta)
 468 {
 469         struct rtattr *tb[TCA_EMATCH_TREE_MAX+1];
 470         struct tcf_ematch_tree_hdr *hdr;
 471
 472         if (parse_rtattr_nested(tb, TCA_EMATCH_TREE_MAX, rta) < 0)
 473                 return -1;
 474
 475         if (tb[TCA_EMATCH_TREE_HDR] == NULL) {
 476                 fprintf(stderr, "Missing ematch tree header\n");
 477                 return -1;
 478         }
 479
 480         if (tb[TCA_EMATCH_TREE_LIST] == NULL) {
 481                 fprintf(stderr, "Missing ematch tree list\n");
 482                 return -1;
 483         }
 484
 485         if (RTA_PAYLOAD(tb[TCA_EMATCH_TREE_HDR]) < sizeof(*hdr)) {
 486                 fprintf(stderr, "Ematch tree header size mismatch\n");
 487                 return -1;
 488         }
 489
 490         hdr = RTA_DATA(tb[TCA_EMATCH_TREE_HDR]);
 491
 492         return print_ematch_list(fd, hdr, tb[TCA_EMATCH_TREE_LIST]);
 493 }
 494
 495 struct bstr * bstr_alloc(const char *text)
 496 {
 497         struct bstr *b = calloc(1, sizeof(*b));
 498
 499         if (b == NULL)
 500                 return NULL;
 501
 502         b->data = strdup(text);
 503         if (b->data == NULL) {
 504                 free(b);
 505                 return NULL;
 506         }
 507
 508         b->len = strlen(text);
 509
 510         return b;
 511 }
 512
 513 unsigned long bstrtoul(const struct bstr *b)
 514 {
 515         char *inv = NULL;
 516         unsigned long l;
 517         char buf[b->len+1];
 518
 519         memcpy(buf, b->data, b->len);
 520         buf[b->len] = '\0';
 521
 522         l = strtoul(buf, &inv, 0);
 523         if (l == ULONG_MAX || inv == buf)
 524                 return ULONG_MAX;
 525
 526         return l;
 527 }
 528
 529 void bstr_print(FILE *fd, const struct bstr *b, int ascii)
 530 {
 531         int i;
 532         char *s = b->data;
 533
 534         if (ascii)
 535                 for (i = 0; i < b->len; i++)
 536                     fprintf(fd, "%c", isprint(s[i]) ? s[i] : '.');
 537         else {
 538                 for (i = 0; i < b->len; i++)
 539                     fprintf(fd, "%02x", s[i]);
 540                 fprintf(fd, "\"");
 541                 for (i = 0; i < b->len; i++)
 542                     fprintf(fd, "%c", isprint(s[i]) ? s[i] : '.');
 543                 fprintf(fd, "\"");
 544         }
 545 }
 546
 547 void print_ematch_tree(const struct ematch *tree)
 548 {
 549         const struct ematch *t;
 550
 551         for (t = tree; t; t = t->next) {
 552                 if (t->inverted)
 553                         printf("NOT ");
 554
 555                 if (t->child) {
 556                         printf("(");
 557                         print_ematch_tree(t->child);
 558                         printf(")");
 559                 } else {
 560                         struct bstr *b;
 561                         for (b = t->args; b; b = b->next)
 562                                 printf("%s%s", b->data, b->next ? " " : "");
 563                 }
 564
 565                 if (t->relation == TCF_EM_REL_AND)
 566                         printf(" AND ");
 567                 else if (t->relation == TCF_EM_REL_OR)
 568                         printf(" OR ");
 569         }
 570 }