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;
37 struct ematch *ematch_root;
38
39 static int begin_argc;
40 static char **begin_argv;
41
map_warning(int num,char * kind)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
lookup_map(__u16 num,char * dst,int len,const char * file)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
lookup_map_id(char * kind,int * dst,const char * file)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
get_ematch_kind(char * kind)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
get_ematch_kind_num(__u16 kind)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
parse_tree(struct nlmsghdr * n,struct ematch * tree)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 = { .flags = t->relation };
181
182 if (t->inverted)
183 hdr.flags |= TCF_EM_INVERT;
184
185 addattr_l(n, MAX_MSG, index++, NULL, 0);
186
187 if (t->child) {
188 __u32 r = t->child_ref;
189
190 addraw_l(n, MAX_MSG, &hdr, sizeof(hdr));
191 addraw_l(n, MAX_MSG, &r, sizeof(r));
192 } else {
193 int num = 0, err;
194 char buf[64];
195 struct ematch_util *e;
196
197 if (t->args == NULL)
198 return -1;
199
200 strncpy(buf, (char *) t->args->data, sizeof(buf)-1);
201 e = get_ematch_kind(buf);
202 if (e == NULL) {
203 fprintf(stderr, "Unknown ematch \"%s\"\n",
204 buf);
205 return -1;
206 }
207
208 err = lookup_map_id(buf, &num, EMATCH_MAP);
209 if (err < 0) {
210 if (err == -ENOENT)
211 map_warning(e->kind_num, buf);
212 return err;
213 }
214
215 hdr.kind = num;
216 if (e->parse_eopt(n, &hdr, t->args->next) < 0)
217 return -1;
218 }
219
220 tail->rta_len = (void *) NLMSG_TAIL(n) - (void *) tail;
221 }
222
223 return 0;
224 }
225
flatten_tree(struct ematch * head,struct ematch * tree)226 static int flatten_tree(struct ematch *head, struct ematch *tree)
227 {
228 int i, count = 0;
229 struct ematch *t;
230
231 for (;;) {
232 count++;
233
234 if (tree->child) {
235 for (t = head; t->next; t = t->next);
236 t->next = tree->child;
237 count += flatten_tree(head, tree->child);
238 }
239
240 if (tree->relation == 0)
241 break;
242
243 tree = tree->next;
244 }
245
246 for (i = 0, t = head; t; t = t->next, i++)
247 t->index = i;
248
249 for (t = head; t; t = t->next)
250 if (t->child)
251 t->child_ref = t->child->index;
252
253 return count;
254 }
255
em_parse_error(int err,struct bstr * args,struct bstr * carg,struct ematch_util * e,char * fmt,...)256 int em_parse_error(int err, struct bstr *args, struct bstr *carg,
257 struct ematch_util *e, char *fmt, ...)
258 {
259 va_list a;
260
261 va_start(a, fmt);
262 vfprintf(stderr, fmt, a);
263 va_end(a);
264
265 if (ematch_err)
266 fprintf(stderr, ": %s\n... ", ematch_err);
267 else
268 fprintf(stderr, "\n... ");
269
270 while (ematch_argc < begin_argc) {
271 if (ematch_argc == (begin_argc - 1))
272 fprintf(stderr, ">>%s<< ", *begin_argv);
273 else
274 fprintf(stderr, "%s ", *begin_argv);
275 begin_argv++;
276 begin_argc--;
277 }
278
279 fprintf(stderr, "...\n");
280
281 if (args) {
282 fprintf(stderr, "... %s(", e->kind);
283 while (args) {
284 fprintf(stderr, "%s", args == carg ? ">>" : "");
285 bstr_print(stderr, args, 1);
286 fprintf(stderr, "%s%s", args == carg ? "<<" : "",
287 args->next ? " " : "");
288 args = args->next;
289 }
290 fprintf(stderr, ")...\n");
291
292 }
293
294 if (e == NULL) {
295 fprintf(stderr,
296 "Usage: EXPR\n" \
297 "where: EXPR := TERM [ { and | or } EXPR ]\n" \
298 " TERM := [ not ] { MATCH | '(' EXPR ')' }\n" \
299 " MATCH := module '(' ARGS ')'\n" \
300 " ARGS := ARG1 ARG2 ...\n" \
301 "\n" \
302 "Example: a(x y) and not (b(x) or c(x y z))\n");
303 } else
304 e->print_usage(stderr);
305
306 return -err;
307 }
308
free_ematch_err(void)309 static inline void free_ematch_err(void)
310 {
311 if (ematch_err) {
312 free(ematch_err);
313 ematch_err = NULL;
314 }
315 }
316
317 extern int ematch_parse(void);
318
parse_ematch(int * argc_p,char *** argv_p,int tca_id,struct nlmsghdr * n)319 int parse_ematch(int *argc_p, char ***argv_p, int tca_id, struct nlmsghdr *n)
320 {
321 begin_argc = ematch_argc = *argc_p;
322 begin_argv = ematch_argv = *argv_p;
323
324 if (ematch_parse()) {
325 int err = em_parse_error(EINVAL, NULL, NULL, NULL,
326 "Parse error");
327 free_ematch_err();
328 return err;
329 }
330
331 free_ematch_err();
332
333 /* undo look ahead by parser */
334 ematch_argc++;
335 ematch_argv--;
336
337 if (ematch_root) {
338 struct rtattr *tail, *tail_list;
339
340 struct tcf_ematch_tree_hdr hdr = {
341 .nmatches = flatten_tree(ematch_root, ematch_root),
342 .progid = TCF_EM_PROG_TC
343 };
344
345 tail = NLMSG_TAIL(n);
346 addattr_l(n, MAX_MSG, tca_id, NULL, 0);
347 addattr_l(n, MAX_MSG, TCA_EMATCH_TREE_HDR, &hdr, sizeof(hdr));
348
349 tail_list = NLMSG_TAIL(n);
350 addattr_l(n, MAX_MSG, TCA_EMATCH_TREE_LIST, NULL, 0);
351
352 if (parse_tree(n, ematch_root) < 0)
353 return -1;
354
355 tail_list->rta_len = (void *) NLMSG_TAIL(n) - (void *) tail_list;
356 tail->rta_len = (void *) NLMSG_TAIL(n) - (void *) tail;
357 }
358
359 *argc_p = ematch_argc;
360 *argv_p = ematch_argv;
361
362 return 0;
363 }
364
print_ematch_seq(FILE * fd,struct rtattr ** tb,int start,int prefix)365 static int print_ematch_seq(FILE *fd, struct rtattr **tb, int start,
366 int prefix)
367 {
368 int n, i = start;
369 struct tcf_ematch_hdr *hdr;
370 int dlen;
371 void *data;
372
373 for (;;) {
374 if (tb[i] == NULL)
375 return -1;
376
377 dlen = RTA_PAYLOAD(tb[i]) - sizeof(*hdr);
378 data = (void *) RTA_DATA(tb[i]) + sizeof(*hdr);
379
380 if (dlen < 0)
381 return -1;
382
383 hdr = RTA_DATA(tb[i]);
384
385 if (hdr->flags & TCF_EM_INVERT)
386 fprintf(fd, "NOT ");
387
388 if (hdr->kind == 0) {
389 __u32 ref;
390
391 if (dlen < sizeof(__u32))
392 return -1;
393
394 ref = *(__u32 *) data;
395 fprintf(fd, "(\n");
396 for (n = 0; n <= prefix; n++)
397 fprintf(fd, " ");
398 if (print_ematch_seq(fd, tb, ref + 1, prefix + 1) < 0)
399 return -1;
400 for (n = 0; n < prefix; n++)
401 fprintf(fd, " ");
402 fprintf(fd, ") ");
403
404 } else {
405 struct ematch_util *e;
406
407 e = get_ematch_kind_num(hdr->kind);
408 if (e == NULL)
409 fprintf(fd, "[unknown ematch %d]\n",
410 hdr->kind);
411 else {
412 fprintf(fd, "%s(", e->kind);
413 if (e->print_eopt(fd, hdr, data, dlen) < 0)
414 return -1;
415 fprintf(fd, ")\n");
416 }
417 if (hdr->flags & TCF_EM_REL_MASK)
418 for (n = 0; n < prefix; n++)
419 fprintf(fd, " ");
420 }
421
422 switch (hdr->flags & TCF_EM_REL_MASK) {
423 case TCF_EM_REL_AND:
424 fprintf(fd, "AND ");
425 break;
426
427 case TCF_EM_REL_OR:
428 fprintf(fd, "OR ");
429 break;
430
431 default:
432 return 0;
433 }
434
435 i++;
436 }
437
438 return 0;
439 }
440
print_ematch_list(FILE * fd,struct tcf_ematch_tree_hdr * hdr,struct rtattr * rta)441 static int print_ematch_list(FILE *fd, struct tcf_ematch_tree_hdr *hdr,
442 struct rtattr *rta)
443 {
444 int err = -1;
445 struct rtattr **tb;
446
447 tb = malloc((hdr->nmatches + 1) * sizeof(struct rtattr *));
448 if (tb == NULL)
449 return -1;
450
451 if (hdr->nmatches > 0) {
452 if (parse_rtattr_nested(tb, hdr->nmatches, rta) < 0)
453 goto errout;
454
455 fprintf(fd, "\n ");
456 if (print_ematch_seq(fd, tb, 1, 1) < 0)
457 goto errout;
458 }
459
460 err = 0;
461 errout:
462 free(tb);
463 return err;
464 }
465
print_ematch(FILE * fd,const struct rtattr * rta)466 int print_ematch(FILE *fd, const struct rtattr *rta)
467 {
468 struct rtattr *tb[TCA_EMATCH_TREE_MAX+1];
469 struct tcf_ematch_tree_hdr *hdr;
470
471 if (parse_rtattr_nested(tb, TCA_EMATCH_TREE_MAX, rta) < 0)
472 return -1;
473
474 if (tb[TCA_EMATCH_TREE_HDR] == NULL) {
475 fprintf(stderr, "Missing ematch tree header\n");
476 return -1;
477 }
478
479 if (tb[TCA_EMATCH_TREE_LIST] == NULL) {
480 fprintf(stderr, "Missing ematch tree list\n");
481 return -1;
482 }
483
484 if (RTA_PAYLOAD(tb[TCA_EMATCH_TREE_HDR]) < sizeof(*hdr)) {
485 fprintf(stderr, "Ematch tree header size mismatch\n");
486 return -1;
487 }
488
489 hdr = RTA_DATA(tb[TCA_EMATCH_TREE_HDR]);
490
491 return print_ematch_list(fd, hdr, tb[TCA_EMATCH_TREE_LIST]);
492 }
493
bstr_alloc(const char * text)494 struct bstr *bstr_alloc(const char *text)
495 {
496 struct bstr *b = calloc(1, sizeof(*b));
497
498 if (b == NULL)
499 return NULL;
500
501 b->data = strdup(text);
502 if (b->data == NULL) {
503 free(b);
504 return NULL;
505 }
506
507 b->len = strlen(text);
508
509 return b;
510 }
511
bstrtoul(const struct bstr * b)512 unsigned long bstrtoul(const struct bstr *b)
513 {
514 char *inv = NULL;
515 unsigned long l;
516 char buf[b->len+1];
517
518 memcpy(buf, b->data, b->len);
519 buf[b->len] = '\0';
520
521 l = strtoul(buf, &inv, 0);
522 if (l == ULONG_MAX || inv == buf)
523 return ULONG_MAX;
524
525 return l;
526 }
527
bstr_print(FILE * fd,const struct bstr * b,int ascii)528 void bstr_print(FILE *fd, const struct bstr *b, int ascii)
529 {
530 int i;
531 char *s = b->data;
532
533 if (ascii)
534 for (i = 0; i < b->len; i++)
535 fprintf(fd, "%c", isprint(s[i]) ? s[i] : '.');
536 else {
537 for (i = 0; i < b->len; i++)
538 fprintf(fd, "%02x", s[i]);
539 fprintf(fd, "\"");
540 for (i = 0; i < b->len; i++)
541 fprintf(fd, "%c", isprint(s[i]) ? s[i] : '.');
542 fprintf(fd, "\"");
543 }
544 }
545
print_ematch_tree(const struct ematch * tree)546 void print_ematch_tree(const struct ematch *tree)
547 {
548 const struct ematch *t;
549
550 for (t = tree; t; t = t->next) {
551 if (t->inverted)
552 printf("NOT ");
553
554 if (t->child) {
555 printf("(");
556 print_ematch_tree(t->child);
557 printf(")");
558 } else {
559 struct bstr *b;
560
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 }
571