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