• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 /*
2  * q_u32.c		U32 filter.
3  *
4  *		This program is free software; you can u32istribute 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:	Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
10  *		Match mark added by Catalin(ux aka Dino) BOIE <catab at umbrella.ro> [5 nov 2004]
11  *
12  */
13 
14 #include <stdio.h>
15 #include <stdlib.h>
16 #include <unistd.h>
17 #include <syslog.h>
18 #include <fcntl.h>
19 #include <sys/socket.h>
20 #include <netinet/in.h>
21 #include <arpa/inet.h>
22 #include <string.h>
23 #include <linux/if.h>
24 #include <linux/if_ether.h>
25 
26 #include "utils.h"
27 #include "tc_util.h"
28 
29 extern int show_pretty;
30 
explain(void)31 static void explain(void)
32 {
33 	fprintf(stderr, "Usage: ... u32 [ match SELECTOR ... ] [ link HTID ]"
34 		" [ classid CLASSID ]\n");
35 	fprintf(stderr, "               [ action ACTION_SPEC ]"
36 		" [ offset OFFSET_SPEC ]\n");
37 	fprintf(stderr, "               [ ht HTID ] [ hashkey HASHKEY_SPEC ]\n");
38 	fprintf(stderr, "               [ sample SAMPLE ]\n");
39 	fprintf(stderr, "or         u32 divisor DIVISOR\n");
40 	fprintf(stderr, "\n");
41 	fprintf(stderr, "Where: SELECTOR := SAMPLE SAMPLE ...\n");
42 	fprintf(stderr, "       SAMPLE := { ip | ip6 | udp | tcp | icmp |"
43 		" u{32|16|8} | mark } SAMPLE_ARGS [divisor DIVISOR]\n");
44 	fprintf(stderr, "       FILTERID := X:Y:Z\n");
45 	fprintf(stderr, "\nNOTE: CLASSID is parsed at hexadecimal input.\n");
46 }
47 
get_u32_handle(__u32 * handle,const char * str)48 static int get_u32_handle(__u32 *handle, const char *str)
49 {
50 	__u32 htid=0, hash=0, nodeid=0;
51 	char *tmp = strchr(str, ':');
52 
53 	if (tmp == NULL) {
54 		if (memcmp("0x", str, 2) == 0)
55 			return get_u32(handle, str, 16);
56 		return -1;
57 	}
58 	htid = strtoul(str, &tmp, 16);
59 	if (tmp == str && *str != ':' && *str != 0)
60 		return -1;
61 	if (htid>=0x1000)
62 		return -1;
63 	if (*tmp) {
64 		str = tmp + 1;
65 		hash = strtoul(str, &tmp, 16);
66 		if (tmp == str && *str != ':' && *str != 0)
67 			return -1;
68 		if (hash>=0x100)
69 			return -1;
70 		if (*tmp) {
71 			str = tmp + 1;
72 			nodeid = strtoul(str, &tmp, 16);
73 			if (tmp == str && *str != 0)
74 				return -1;
75 			if (nodeid>=0x1000)
76 				return -1;
77 		}
78 	}
79 	*handle = (htid<<20)|(hash<<12)|nodeid;
80 	return 0;
81 }
82 
sprint_u32_handle(__u32 handle,char * buf)83 static char * sprint_u32_handle(__u32 handle, char *buf)
84 {
85 	int bsize = SPRINT_BSIZE-1;
86 	__u32 htid = TC_U32_HTID(handle);
87 	__u32 hash = TC_U32_HASH(handle);
88 	__u32 nodeid = TC_U32_NODE(handle);
89 	char *b = buf;
90 
91 	if (handle == 0) {
92 		snprintf(b, bsize, "none");
93 		return b;
94 	}
95 	if (htid) {
96 		int l = snprintf(b, bsize, "%x:", htid>>20);
97 		bsize -= l;
98 		b += l;
99 	}
100 	if (nodeid|hash) {
101 		if (hash) {
102 			int l = snprintf(b, bsize, "%x", hash);
103 			bsize -= l;
104 			b += l;
105 		}
106 		if (nodeid) {
107 			int l = snprintf(b, bsize, ":%x", nodeid);
108 			bsize -= l;
109 			b += l;
110 		}
111 	}
112 	if (show_raw)
113 		snprintf(b, bsize, "[%08x] ", handle);
114 	return buf;
115 }
116 
pack_key(struct tc_u32_sel * sel,__u32 key,__u32 mask,int off,int offmask)117 static int pack_key(struct tc_u32_sel *sel, __u32 key, __u32 mask,
118 		    int off, int offmask)
119 {
120 	int i;
121 	int hwm = sel->nkeys;
122 
123 	key &= mask;
124 
125 	for (i=0; i<hwm; i++) {
126 		if (sel->keys[i].off == off && sel->keys[i].offmask == offmask) {
127 			__u32 intersect = mask & sel->keys[i].mask;
128 
129 			if ((key ^ sel->keys[i].val) & intersect)
130 				return -1;
131 			sel->keys[i].val |= key;
132 			sel->keys[i].mask |= mask;
133 			return 0;
134 		}
135 	}
136 
137 	if (hwm >= 128)
138 		return -1;
139 	if (off % 4)
140 		return -1;
141 	sel->keys[hwm].val = key;
142 	sel->keys[hwm].mask = mask;
143 	sel->keys[hwm].off = off;
144 	sel->keys[hwm].offmask = offmask;
145 	sel->nkeys++;
146 	return 0;
147 }
148 
pack_key32(struct tc_u32_sel * sel,__u32 key,__u32 mask,int off,int offmask)149 static int pack_key32(struct tc_u32_sel *sel, __u32 key, __u32 mask,
150 		      int off, int offmask)
151 {
152 	key = htonl(key);
153 	mask = htonl(mask);
154 	return pack_key(sel, key, mask, off, offmask);
155 }
156 
pack_key16(struct tc_u32_sel * sel,__u32 key,__u32 mask,int off,int offmask)157 static int pack_key16(struct tc_u32_sel *sel, __u32 key, __u32 mask,
158 		      int off, int offmask)
159 {
160 	if (key > 0xFFFF || mask > 0xFFFF)
161 		return -1;
162 
163 	if ((off & 3) == 0) {
164 		key <<= 16;
165 		mask <<= 16;
166 	}
167 	off &= ~3;
168 	key = htonl(key);
169 	mask = htonl(mask);
170 
171 	return pack_key(sel, key, mask, off, offmask);
172 }
173 
pack_key8(struct tc_u32_sel * sel,__u32 key,__u32 mask,int off,int offmask)174 static int pack_key8(struct tc_u32_sel *sel, __u32 key, __u32 mask, int off, int offmask)
175 {
176 	if (key > 0xFF || mask > 0xFF)
177 		return -1;
178 
179 	if ((off & 3) == 0) {
180 		key <<= 24;
181 		mask <<= 24;
182 	} else if ((off & 3) == 1) {
183 		key <<= 16;
184 		mask <<= 16;
185 	} else if ((off & 3) == 2) {
186 		key <<= 8;
187 		mask <<= 8;
188 	}
189 	off &= ~3;
190 	key = htonl(key);
191 	mask = htonl(mask);
192 
193 	return pack_key(sel, key, mask, off, offmask);
194 }
195 
196 
parse_at(int * argc_p,char *** argv_p,int * off,int * offmask)197 static int parse_at(int *argc_p, char ***argv_p, int *off, int *offmask)
198 {
199 	int argc = *argc_p;
200 	char **argv = *argv_p;
201 	char *p = *argv;
202 
203 	if (argc <= 0)
204 		return -1;
205 
206 	if (strlen(p) > strlen("nexthdr+") &&
207 	    memcmp(p, "nexthdr+", strlen("nexthdr+")) == 0) {
208 		*offmask = -1;
209 		p += strlen("nexthdr+");
210 	} else if (matches(*argv, "nexthdr+") == 0) {
211 		NEXT_ARG();
212 		*offmask = -1;
213 		p = *argv;
214 	}
215 
216 	if (get_integer(off, p, 0))
217 		return -1;
218 	argc--; argv++;
219 
220 	*argc_p = argc;
221 	*argv_p = argv;
222 	return 0;
223 }
224 
225 
parse_u32(int * argc_p,char *** argv_p,struct tc_u32_sel * sel,int off,int offmask)226 static int parse_u32(int *argc_p, char ***argv_p, struct tc_u32_sel *sel,
227 		     int off, int offmask)
228 {
229 	int res = -1;
230 	int argc = *argc_p;
231 	char **argv = *argv_p;
232 	__u32 key;
233 	__u32 mask;
234 
235 	if (argc < 2)
236 		return -1;
237 
238 	if (get_u32(&key, *argv, 0))
239 		return -1;
240 	argc--; argv++;
241 
242 	if (get_u32(&mask, *argv, 16))
243 		return -1;
244 	argc--; argv++;
245 
246 	if (argc > 0 && strcmp(argv[0], "at") == 0) {
247 		NEXT_ARG();
248 		if (parse_at(&argc, &argv, &off, &offmask))
249 			return -1;
250 	}
251 
252 	res = pack_key32(sel, key, mask, off, offmask);
253 	*argc_p = argc;
254 	*argv_p = argv;
255 	return res;
256 }
257 
parse_u16(int * argc_p,char *** argv_p,struct tc_u32_sel * sel,int off,int offmask)258 static int parse_u16(int *argc_p, char ***argv_p, struct tc_u32_sel *sel,
259 		     int off, int offmask)
260 {
261 	int res = -1;
262 	int argc = *argc_p;
263 	char **argv = *argv_p;
264 	__u32 key;
265 	__u32 mask;
266 
267 	if (argc < 2)
268 		return -1;
269 
270 	if (get_u32(&key, *argv, 0))
271 		return -1;
272 	argc--; argv++;
273 
274 	if (get_u32(&mask, *argv, 16))
275 		return -1;
276 	argc--; argv++;
277 
278 	if (argc > 0 && strcmp(argv[0], "at") == 0) {
279 		NEXT_ARG();
280 		if (parse_at(&argc, &argv, &off, &offmask))
281 			return -1;
282 	}
283 	res = pack_key16(sel, key, mask, off, offmask);
284 	*argc_p = argc;
285 	*argv_p = argv;
286 	return res;
287 }
288 
parse_u8(int * argc_p,char *** argv_p,struct tc_u32_sel * sel,int off,int offmask)289 static int parse_u8(int *argc_p, char ***argv_p, struct tc_u32_sel *sel,
290 		    int off, int offmask)
291 {
292 	int res = -1;
293 	int argc = *argc_p;
294 	char **argv = *argv_p;
295 	__u32 key;
296 	__u32 mask;
297 
298 	if (argc < 2)
299 		return -1;
300 
301 	if (get_u32(&key, *argv, 0))
302 		return -1;
303 	argc--; argv++;
304 
305 	if (get_u32(&mask, *argv, 16))
306 		return -1;
307 	argc--; argv++;
308 
309 	if (key > 0xFF || mask > 0xFF)
310 		return -1;
311 
312 	if (argc > 0 && strcmp(argv[0], "at") == 0) {
313 		NEXT_ARG();
314 		if (parse_at(&argc, &argv, &off, &offmask))
315 			return -1;
316 	}
317 
318 	res = pack_key8(sel, key, mask, off, offmask);
319 	*argc_p = argc;
320 	*argv_p = argv;
321 	return res;
322 }
323 
parse_ip_addr(int * argc_p,char *** argv_p,struct tc_u32_sel * sel,int off)324 static int parse_ip_addr(int *argc_p, char ***argv_p, struct tc_u32_sel *sel,
325 			 int off)
326 {
327 	int res = -1;
328 	int argc = *argc_p;
329 	char **argv = *argv_p;
330 	inet_prefix addr;
331 	__u32 mask;
332 	int offmask = 0;
333 
334 	if (argc < 1)
335 		return -1;
336 
337 	if (get_prefix_1(&addr, *argv, AF_INET))
338 		return -1;
339 	argc--; argv++;
340 
341 	if (argc > 0 && strcmp(argv[0], "at") == 0) {
342 		NEXT_ARG();
343 		if (parse_at(&argc, &argv, &off, &offmask))
344 			return -1;
345 	}
346 
347 	mask = 0;
348 	if (addr.bitlen)
349 		mask = htonl(0xFFFFFFFF << (32 - addr.bitlen));
350 	if (pack_key(sel, addr.data[0], mask, off, offmask) < 0)
351 		return -1;
352 	res = 0;
353 
354 	*argc_p = argc;
355 	*argv_p = argv;
356 	return res;
357 }
358 
parse_ip6_addr(int * argc_p,char *** argv_p,struct tc_u32_sel * sel,int off)359 static int parse_ip6_addr(int *argc_p, char ***argv_p,
360 			  struct tc_u32_sel *sel, int off)
361 {
362 	int res = -1;
363 	int argc = *argc_p;
364 	char **argv = *argv_p;
365 	int plen = 128;
366 	int i;
367 	inet_prefix addr;
368 	int offmask = 0;
369 
370 	if (argc < 1)
371 		return -1;
372 
373 	if (get_prefix_1(&addr, *argv, AF_INET6))
374 		return -1;
375 	argc--; argv++;
376 
377 	if (argc > 0 && strcmp(argv[0], "at") == 0) {
378 		NEXT_ARG();
379 		if (parse_at(&argc, &argv, &off, &offmask))
380 			return -1;
381 	}
382 
383 	plen = addr.bitlen;
384 	for (i = 0; i < plen; i += 32) {
385 //		if (((i + 31) & ~0x1F) <= plen) {
386 		if (i + 31 <= plen) {
387 			res = pack_key(sel, addr.data[i / 32],
388 				       0xFFFFFFFF, off + 4 * (i / 32), offmask);
389 			if (res < 0)
390 				return -1;
391 		} else if (i < plen) {
392 			__u32 mask = htonl(0xFFFFFFFF << (32 - (plen - i)));
393 			res = pack_key(sel, addr.data[i / 32],
394 				       mask, off + 4 * (i / 32), offmask);
395 			if (res < 0)
396 				return -1;
397 		}
398 	}
399 	res = 0;
400 
401 	*argc_p = argc;
402 	*argv_p = argv;
403 	return res;
404 }
405 
parse_ip6_class(int * argc_p,char *** argv_p,struct tc_u32_sel * sel)406 static int parse_ip6_class(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
407 {
408 	int res = -1;
409 	int argc = *argc_p;
410 	char **argv = *argv_p;
411 	__u32 key;
412 	__u32 mask;
413 	int off = 0;
414 	int offmask = 0;
415 
416 	if (argc < 2)
417 		return -1;
418 
419 	if (get_u32(&key, *argv, 0))
420 		return -1;
421 	argc--; argv++;
422 
423 	if (get_u32(&mask, *argv, 16))
424 		return -1;
425 	argc--; argv++;
426 
427 	if (key > 0xFF || mask > 0xFF)
428 		return -1;
429 
430 	key <<= 20;
431 	mask <<= 20;
432 	key = htonl(key);
433 	mask = htonl(mask);
434 
435 	res = pack_key(sel, key, mask, off, offmask);
436 	if (res < 0)
437 		return -1;
438 
439 	*argc_p = argc;
440 	*argv_p = argv;
441 	return 0;
442 }
443 
parse_ether_addr(int * argc_p,char *** argv_p,struct tc_u32_sel * sel,int off)444 static int parse_ether_addr(int *argc_p, char ***argv_p,
445 			    struct tc_u32_sel *sel, int off)
446 {
447 	int res = -1;
448 	int argc = *argc_p;
449 	char **argv = *argv_p;
450 	__u8 addr[6];
451 	int offmask = 0;
452 	int i;
453 
454 	if (argc < 1)
455 		return -1;
456 
457 	if (sscanf(*argv, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
458 		   addr + 0, addr + 1, addr + 2,
459 		   addr + 3, addr + 4, addr + 5) != 6) {
460 		fprintf(stderr, "parse_ether_addr: improperly formed address '%s'\n",
461 			*argv);
462 		return -1;
463 	}
464 
465 	argc--; argv++;
466 	if (argc > 0 && strcmp(argv[0], "at") == 0) {
467 		NEXT_ARG();
468 		if (parse_at(&argc, &argv, &off, &offmask))
469 			return -1;
470 	}
471 
472 	for (i = 0; i < 6; i++) {
473 		res = pack_key8(sel, addr[i], 0xFF, off + i, offmask);
474 		if (res < 0)
475 			return -1;
476 	}
477 
478 	*argc_p = argc;
479 	*argv_p = argv;
480 	return res;
481 }
482 
parse_ip(int * argc_p,char *** argv_p,struct tc_u32_sel * sel)483 static int parse_ip(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
484 {
485 	int res = -1;
486 	int argc = *argc_p;
487 	char **argv = *argv_p;
488 
489 	if (argc < 2)
490 		return -1;
491 
492 	if (strcmp(*argv, "src") == 0) {
493 		NEXT_ARG();
494 		res = parse_ip_addr(&argc, &argv, sel, 12);
495 	} else if (strcmp(*argv, "dst") == 0) {
496 		NEXT_ARG();
497 		res = parse_ip_addr(&argc, &argv, sel, 16);
498 	} else if (strcmp(*argv, "tos") == 0 ||
499 	    matches(*argv, "dsfield") == 0 ||
500 	    matches(*argv, "precedence") == 0) {
501 		NEXT_ARG();
502 		res = parse_u8(&argc, &argv, sel, 1, 0);
503 	} else if (strcmp(*argv, "ihl") == 0) {
504 		NEXT_ARG();
505 		res = parse_u8(&argc, &argv, sel, 0, 0);
506 	} else if (strcmp(*argv, "protocol") == 0) {
507 		NEXT_ARG();
508 		res = parse_u8(&argc, &argv, sel, 9, 0);
509 	} else if (strcmp(*argv, "nofrag") == 0) {
510 		argc--; argv++;
511 		res = pack_key16(sel, 0, 0x3FFF, 6, 0);
512 	} else if (strcmp(*argv, "firstfrag") == 0) {
513 		argc--; argv++;
514 		res = pack_key16(sel, 0x2000, 0x3FFF, 6, 0);
515 	} else if (strcmp(*argv, "df") == 0) {
516 		argc--; argv++;
517 		res = pack_key16(sel, 0x4000, 0x4000, 6, 0);
518 	} else if (strcmp(*argv, "mf") == 0) {
519 		argc--; argv++;
520 		res = pack_key16(sel, 0x2000, 0x2000, 6, 0);
521 	} else if (strcmp(*argv, "dport") == 0) {
522 		NEXT_ARG();
523 		res = parse_u16(&argc, &argv, sel, 22, 0);
524 	} else if (strcmp(*argv, "sport") == 0) {
525 		NEXT_ARG();
526 		res = parse_u16(&argc, &argv, sel, 20, 0);
527 	} else if (strcmp(*argv, "icmp_type") == 0) {
528 		NEXT_ARG();
529 		res = parse_u8(&argc, &argv, sel, 20, 0);
530 	} else if (strcmp(*argv, "icmp_code") == 0) {
531 		NEXT_ARG();
532 		res = parse_u8(&argc, &argv, sel, 21, 0);
533 	} else
534 		return -1;
535 
536 	*argc_p = argc;
537 	*argv_p = argv;
538 	return res;
539 }
540 
parse_ip6(int * argc_p,char *** argv_p,struct tc_u32_sel * sel)541 static int parse_ip6(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
542 {
543 	int res = -1;
544 	int argc = *argc_p;
545 	char **argv = *argv_p;
546 
547 	if (argc < 2)
548 		return -1;
549 
550 	if (strcmp(*argv, "src") == 0) {
551 		NEXT_ARG();
552 		res = parse_ip6_addr(&argc, &argv, sel, 8);
553 	} else if (strcmp(*argv, "dst") == 0) {
554 		NEXT_ARG();
555 		res = parse_ip6_addr(&argc, &argv, sel, 24);
556 	} else if (strcmp(*argv, "priority") == 0) {
557 		NEXT_ARG();
558 		res = parse_ip6_class(&argc, &argv, sel);
559 	} else if (strcmp(*argv, "protocol") == 0) {
560 		NEXT_ARG();
561 		res = parse_u8(&argc, &argv, sel, 6, 0);
562 	} else if (strcmp(*argv, "flowlabel") == 0) {
563 		NEXT_ARG();
564 		res = parse_u32(&argc, &argv, sel, 0, 0);
565 	} else if (strcmp(*argv, "dport") == 0) {
566 		NEXT_ARG();
567 		res = parse_u16(&argc, &argv, sel, 42, 0);
568 	} else if (strcmp(*argv, "sport") == 0) {
569 		NEXT_ARG();
570 		res = parse_u16(&argc, &argv, sel, 40, 0);
571 	} else if (strcmp(*argv, "icmp_type") == 0) {
572 		NEXT_ARG();
573 		res = parse_u8(&argc, &argv, sel, 40, 0);
574 	} else if (strcmp(*argv, "icmp_code") == 0) {
575 		NEXT_ARG();
576 		res = parse_u8(&argc, &argv, sel, 41, 1);
577 	} else
578 		return -1;
579 
580 	*argc_p = argc;
581 	*argv_p = argv;
582 	return res;
583 }
584 
parse_ether(int * argc_p,char *** argv_p,struct tc_u32_sel * sel)585 static int parse_ether(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
586 {
587 	int res = -1;
588 	int argc = *argc_p;
589 	char **argv = *argv_p;
590 
591 	if (argc < 2)
592 		return -1;
593 
594 	if (strcmp(*argv, "src") == 0) {
595 		NEXT_ARG();
596 		res = parse_ether_addr(&argc, &argv, sel, -8);
597 	} else if (strcmp(*argv, "dst") == 0) {
598 		NEXT_ARG();
599 		res = parse_ether_addr(&argc, &argv, sel, -14);
600 	} else {
601 		fprintf(stderr, "Unknown match: ether %s\n", *argv);
602 		return -1;
603 	}
604 
605 	*argc_p = argc;
606 	*argv_p = argv;
607 	return res;
608 }
609 
610 #define parse_tcp parse_udp
parse_udp(int * argc_p,char *** argv_p,struct tc_u32_sel * sel)611 static int parse_udp(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
612 {
613 	int res = -1;
614 	int argc = *argc_p;
615 	char **argv = *argv_p;
616 
617 	if (argc < 2)
618 		return -1;
619 
620 	if (strcmp(*argv, "src") == 0) {
621 		NEXT_ARG();
622 		res = parse_u16(&argc, &argv, sel, 0, -1);
623 	} else if (strcmp(*argv, "dst") == 0) {
624 		NEXT_ARG();
625 		res = parse_u16(&argc, &argv, sel, 2, -1);
626 	} else
627 		return -1;
628 
629 	*argc_p = argc;
630 	*argv_p = argv;
631 	return res;
632 }
633 
634 
parse_icmp(int * argc_p,char *** argv_p,struct tc_u32_sel * sel)635 static int parse_icmp(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
636 {
637 	int res = -1;
638 	int argc = *argc_p;
639 	char **argv = *argv_p;
640 
641 	if (argc < 2)
642 		return -1;
643 
644 	if (strcmp(*argv, "type") == 0) {
645 		NEXT_ARG();
646 		res = parse_u8(&argc, &argv, sel, 0, -1);
647 	} else if (strcmp(*argv, "code") == 0) {
648 		NEXT_ARG();
649 		res = parse_u8(&argc, &argv, sel, 1, -1);
650 	} else
651 		return -1;
652 
653 	*argc_p = argc;
654 	*argv_p = argv;
655 	return res;
656 }
657 
parse_mark(int * argc_p,char *** argv_p,struct nlmsghdr * n)658 static int parse_mark(int *argc_p, char ***argv_p, struct nlmsghdr *n)
659 {
660 	int res = -1;
661 	int argc = *argc_p;
662 	char **argv = *argv_p;
663 	struct tc_u32_mark mark;
664 
665 	if (argc <= 1)
666 		return -1;
667 
668 	if (get_u32(&mark.val, *argv, 0)) {
669 		fprintf(stderr, "Illegal \"mark\" value\n");
670 		return -1;
671 	}
672 	NEXT_ARG();
673 
674 	if (get_u32(&mark.mask, *argv, 0)) {
675 		fprintf(stderr, "Illegal \"mark\" mask\n");
676 		return -1;
677 	}
678 	NEXT_ARG();
679 
680 	if ((mark.val & mark.mask) != mark.val) {
681 		fprintf(stderr, "Illegal \"mark\" (impossible combination)\n");
682 		return -1;
683 	}
684 
685 	addattr_l(n, MAX_MSG, TCA_U32_MARK, &mark, sizeof(mark));
686 	res = 0;
687 
688 	*argc_p = argc;
689 	*argv_p = argv;
690 	return res;
691 }
692 
parse_selector(int * argc_p,char *** argv_p,struct tc_u32_sel * sel,struct nlmsghdr * n)693 static int parse_selector(int *argc_p, char ***argv_p,
694 			  struct tc_u32_sel *sel, struct nlmsghdr *n)
695 {
696 	int argc = *argc_p;
697 	char **argv = *argv_p;
698 	int res = -1;
699 
700 	if (argc <= 0)
701 		return -1;
702 
703 	if (matches(*argv, "u32") == 0) {
704 		NEXT_ARG();
705 		res = parse_u32(&argc, &argv, sel, 0, 0);
706 	} else if (matches(*argv, "u16") == 0) {
707 		NEXT_ARG();
708 		res = parse_u16(&argc, &argv, sel, 0, 0);
709 	} else if (matches(*argv, "u8") == 0) {
710 		NEXT_ARG();
711 		res = parse_u8(&argc, &argv, sel, 0, 0);
712 	} else if (matches(*argv, "ip") == 0) {
713 		NEXT_ARG();
714 		res = parse_ip(&argc, &argv, sel);
715 	} else 	if (matches(*argv, "ip6") == 0) {
716 		NEXT_ARG();
717 		res = parse_ip6(&argc, &argv, sel);
718 	} else if (matches(*argv, "udp") == 0) {
719 		NEXT_ARG();
720 		res = parse_udp(&argc, &argv, sel);
721 	} else if (matches(*argv, "tcp") == 0) {
722 		NEXT_ARG();
723 		res = parse_tcp(&argc, &argv, sel);
724 	} else if (matches(*argv, "icmp") == 0) {
725 		NEXT_ARG();
726 		res = parse_icmp(&argc, &argv, sel);
727 	} else if (matches(*argv, "mark") == 0) {
728 		NEXT_ARG();
729 		res = parse_mark(&argc, &argv, n);
730 	} else if (matches(*argv, "ether") == 0) {
731 		NEXT_ARG();
732 		res = parse_ether(&argc, &argv, sel);
733 	} else
734 		return -1;
735 
736 	*argc_p = argc;
737 	*argv_p = argv;
738 	return res;
739 }
740 
parse_offset(int * argc_p,char *** argv_p,struct tc_u32_sel * sel)741 static int parse_offset(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
742 {
743 	int argc = *argc_p;
744 	char **argv = *argv_p;
745 
746 	while (argc > 0) {
747 		if (matches(*argv, "plus") == 0) {
748 			int off;
749 			NEXT_ARG();
750 			if (get_integer(&off, *argv, 0))
751 				return -1;
752 			sel->off = off;
753 			sel->flags |= TC_U32_OFFSET;
754 		} else if (matches(*argv, "at") == 0) {
755 			int off;
756 			NEXT_ARG();
757 			if (get_integer(&off, *argv, 0))
758 				return -1;
759 			sel->offoff = off;
760 			if (off%2) {
761 				fprintf(stderr, "offset \"at\" must be even\n");
762 				return -1;
763 			}
764 			sel->flags |= TC_U32_VAROFFSET;
765 		} else if (matches(*argv, "mask") == 0) {
766 			__u16 mask;
767 			NEXT_ARG();
768 			if (get_u16(&mask, *argv, 16))
769 				return -1;
770 			sel->offmask = htons(mask);
771 			sel->flags |= TC_U32_VAROFFSET;
772 		} else if (matches(*argv, "shift") == 0) {
773 			int shift;
774 			NEXT_ARG();
775 			if (get_integer(&shift, *argv, 0))
776 				return -1;
777 			sel->offshift = shift;
778 			sel->flags |= TC_U32_VAROFFSET;
779 		} else if (matches(*argv, "eat") == 0) {
780 			sel->flags |= TC_U32_EAT;
781 		} else {
782 			break;
783 		}
784 		argc--; argv++;
785 	}
786 
787 	*argc_p = argc;
788 	*argv_p = argv;
789 	return 0;
790 }
791 
parse_hashkey(int * argc_p,char *** argv_p,struct tc_u32_sel * sel)792 static int parse_hashkey(int *argc_p, char ***argv_p, struct tc_u32_sel *sel)
793 {
794 	int argc = *argc_p;
795 	char **argv = *argv_p;
796 
797 	while (argc > 0) {
798 		if (matches(*argv, "mask") == 0) {
799 			__u32 mask;
800 			NEXT_ARG();
801 			if (get_u32(&mask, *argv, 16))
802 				return -1;
803 			sel->hmask = htonl(mask);
804 		} else if (matches(*argv, "at") == 0) {
805 			int num;
806 			NEXT_ARG();
807 			if (get_integer(&num, *argv, 0))
808 				return -1;
809 			if (num%4)
810 				return -1;
811 			sel->hoff = num;
812 		} else {
813 			break;
814 		}
815 		argc--; argv++;
816 	}
817 
818 	*argc_p = argc;
819 	*argv_p = argv;
820 	return 0;
821 }
822 
print_ipv4(FILE * f,const struct tc_u32_key * key)823 static void print_ipv4(FILE *f, const struct tc_u32_key *key)
824 {
825 	char abuf[256];
826 
827 	switch (key->off) {
828 	case 0:
829 		switch (ntohl(key->mask)) {
830 		case 0x0f000000:
831 			fprintf(f, "\n  match IP ihl %u", ntohl(key->val) >> 24);
832 			return;
833 		case 0x00ff0000:
834 			fprintf(f, "\n  match IP dsfield %#x", ntohl(key->val) >> 16);
835 			return;
836 		}
837 		break;
838 	case 8:
839 		if (ntohl(key->mask) == 0x00ff0000) {
840 			fprintf(f, "\n  match IP protocol %d", ntohl(key->val) >> 16);
841 			return;
842 		}
843 		break;
844 	case 12:
845 	case 16: {
846 			int bits = mask2bits(key->mask);
847 			if (bits >= 0) {
848 				fprintf(f, "\n  %s %s/%d",
849 					key->off == 12 ? "match IP src" : "match IP dst",
850 					inet_ntop(AF_INET, &key->val,
851 						  abuf, sizeof(abuf)),
852 					bits);
853 				return;
854 			}
855 		}
856 		break;
857 
858 	case 20:
859 		switch (ntohl(key->mask)) {
860 		case 0x0000ffff:
861 			fprintf(f, "\n  match dport %u",
862 				ntohl(key->val) & 0xffff);
863 			return;
864 		case 0xffff0000:
865 			fprintf(f, "\n  match sport %u",
866 				ntohl(key->val) >> 16);
867 			return;
868 		case 0xffffffff:
869 			fprintf(f, "\n  match dport %u, match sport %u",
870 				ntohl(key->val) & 0xffff,
871 				ntohl(key->val) >> 16);
872 
873 			return;
874 		}
875 		/* XXX: Default print_raw */
876 	}
877 }
878 
print_ipv6(FILE * f,const struct tc_u32_key * key)879 static void print_ipv6(FILE *f, const struct tc_u32_key *key)
880 {
881 	char abuf[256];
882 
883 	switch (key->off) {
884 	case 0:
885 		switch (ntohl(key->mask)) {
886 		case 0x0f000000:
887 			fprintf(f, "\n  match IP ihl %u", ntohl(key->val) >> 24);
888 			return;
889 		case 0x00ff0000:
890 			fprintf(f, "\n  match IP dsfield %#x", ntohl(key->val) >> 16);
891 			return;
892 		}
893 		break;
894 	case 8:
895 		if (ntohl(key->mask) == 0x00ff0000) {
896 			fprintf(f, "\n  match IP protocol %d", ntohl(key->val) >> 16);
897 			return;
898 		}
899 		break;
900 	case 12:
901 	case 16: {
902 			int bits = mask2bits(key->mask);
903 			if (bits >= 0) {
904 				fprintf(f, "\n  %s %s/%d",
905 					key->off == 12 ? "match IP src" : "match IP dst",
906 					inet_ntop(AF_INET, &key->val,
907 						  abuf, sizeof(abuf)),
908 					bits);
909 				return;
910 			}
911 		}
912 		break;
913 
914 	case 20:
915 		switch (ntohl(key->mask)) {
916 		case 0x0000ffff:
917 			fprintf(f, "\n  match sport %u",
918 				ntohl(key->val) & 0xffff);
919 			return;
920 		case 0xffff0000:
921 			fprintf(f, "\n  match dport %u",
922 				ntohl(key->val) >> 16);
923 			return;
924 		case 0xffffffff:
925 			fprintf(f, "\n  match sport %u, match dport %u",
926 				ntohl(key->val) & 0xffff,
927 				ntohl(key->val) >> 16);
928 
929 			return;
930 		}
931 		/* XXX: Default print_raw */
932 	}
933 }
934 
print_raw(FILE * f,const struct tc_u32_key * key)935 static void print_raw(FILE *f, const struct tc_u32_key *key)
936 {
937 	fprintf(f, "\n  match %08x/%08x at %s%d",
938 		(unsigned int)ntohl(key->val),
939 		(unsigned int)ntohl(key->mask),
940 		key->offmask ? "nexthdr+" : "",
941 		key->off);
942 }
943 
944 static const struct {
945 	__u16 proto;
946 	__u16 pad;
947 	void (*pprinter)(FILE *f, const struct tc_u32_key *key);
948 } u32_pprinters[] = {
949 	{0, 	   0, print_raw},
950 	{ETH_P_IP, 0, print_ipv4},
951 	{ETH_P_IPV6, 0, print_ipv6},
952 };
953 
show_keys(FILE * f,const struct tc_u32_key * key)954 static void show_keys(FILE *f, const struct tc_u32_key *key)
955 {
956 	int i = 0;
957 
958 	if (!show_pretty)
959 		goto show_k;
960 
961 	for (i = 0; i < sizeof(u32_pprinters) / sizeof(u32_pprinters[0]); i++) {
962 		if (u32_pprinters[i].proto == ntohs(f_proto)) {
963 show_k:
964 			u32_pprinters[i].pprinter(f, key);
965 			return;
966 		}
967 	}
968 
969 	i = 0;
970 	goto show_k;
971 }
972 
u32_parse_opt(struct filter_util * qu,char * handle,int argc,char ** argv,struct nlmsghdr * n)973 static int u32_parse_opt(struct filter_util *qu, char *handle,
974 			 int argc, char **argv, struct nlmsghdr *n)
975 {
976 	struct {
977 		struct tc_u32_sel sel;
978 		struct tc_u32_key keys[128];
979 	} sel;
980 	struct tcmsg *t = NLMSG_DATA(n);
981 	struct rtattr *tail;
982 	int sel_ok = 0, terminal_ok = 0;
983 	int sample_ok = 0;
984 	__u32 htid = 0;
985 	__u32 order = 0;
986 
987 	memset(&sel, 0, sizeof(sel));
988 
989 	if (handle && get_u32_handle(&t->tcm_handle, handle)) {
990 		fprintf(stderr, "Illegal filter ID\n");
991 		return -1;
992 	}
993 
994 	if (argc == 0)
995 		return 0;
996 
997 	tail = NLMSG_TAIL(n);
998 	addattr_l(n, MAX_MSG, TCA_OPTIONS, NULL, 0);
999 
1000 	while (argc > 0) {
1001 		if (matches(*argv, "match") == 0) {
1002 			NEXT_ARG();
1003 			if (parse_selector(&argc, &argv, &sel.sel, n)) {
1004 				fprintf(stderr, "Illegal \"match\"\n");
1005 				return -1;
1006 			}
1007 			sel_ok++;
1008 			continue;
1009 		} else if (matches(*argv, "offset") == 0) {
1010 			NEXT_ARG();
1011 			if (parse_offset(&argc, &argv, &sel.sel)) {
1012 				fprintf(stderr, "Illegal \"offset\"\n");
1013 				return -1;
1014 			}
1015 			continue;
1016 		} else if (matches(*argv, "hashkey") == 0) {
1017 			NEXT_ARG();
1018 			if (parse_hashkey(&argc, &argv, &sel.sel)) {
1019 				fprintf(stderr, "Illegal \"hashkey\"\n");
1020 				return -1;
1021 			}
1022 			continue;
1023 		} else if (matches(*argv, "classid") == 0 ||
1024 			   strcmp(*argv, "flowid") == 0) {
1025 			unsigned handle;
1026 			NEXT_ARG();
1027 			if (get_tc_classid(&handle, *argv)) {
1028 				fprintf(stderr, "Illegal \"classid\"\n");
1029 				return -1;
1030 			}
1031 			addattr_l(n, MAX_MSG, TCA_U32_CLASSID, &handle, 4);
1032 			sel.sel.flags |= TC_U32_TERMINAL;
1033 		} else if (matches(*argv, "divisor") == 0) {
1034 			unsigned divisor;
1035 			NEXT_ARG();
1036 			if (get_unsigned(&divisor, *argv, 0) ||
1037 			    divisor == 0 ||
1038 			    divisor > 0x100 || ((divisor - 1) & divisor)) {
1039 				fprintf(stderr, "Illegal \"divisor\"\n");
1040 				return -1;
1041 			}
1042 			addattr_l(n, MAX_MSG, TCA_U32_DIVISOR, &divisor, 4);
1043 		} else if (matches(*argv, "order") == 0) {
1044 			NEXT_ARG();
1045 			if (get_u32(&order, *argv, 0)) {
1046 				fprintf(stderr, "Illegal \"order\"\n");
1047 				return -1;
1048 			}
1049 		} else if (strcmp(*argv, "link") == 0) {
1050 			unsigned handle;
1051 			NEXT_ARG();
1052 			if (get_u32_handle(&handle, *argv)) {
1053 				fprintf(stderr, "Illegal \"link\"\n");
1054 				return -1;
1055 			}
1056 			if (handle && TC_U32_NODE(handle)) {
1057 				fprintf(stderr, "\"link\" must be a hash table.\n");
1058 				return -1;
1059 			}
1060 			addattr_l(n, MAX_MSG, TCA_U32_LINK, &handle, 4);
1061 		} else if (strcmp(*argv, "ht") == 0) {
1062 			unsigned handle;
1063 			NEXT_ARG();
1064 			if (get_u32_handle(&handle, *argv)) {
1065 				fprintf(stderr, "Illegal \"ht\"\n");
1066 				return -1;
1067 			}
1068 			if (handle && TC_U32_NODE(handle)) {
1069 				fprintf(stderr, "\"ht\" must be a hash table.\n");
1070 				return -1;
1071 			}
1072 			if (sample_ok)
1073 				htid = (htid & 0xFF000) | (handle & 0xFFF00000);
1074 			else
1075 				htid = (handle & 0xFFFFF000);
1076 		} else if (strcmp(*argv, "sample") == 0) {
1077 			__u32 hash;
1078 			unsigned divisor = 0x100;
1079 
1080 			struct {
1081 				struct tc_u32_sel sel;
1082 				struct tc_u32_key keys[4];
1083 			} sel2;
1084 			memset(&sel2, 0, sizeof(sel2));
1085 			NEXT_ARG();
1086 			if (parse_selector(&argc, &argv, &sel2.sel, n)) {
1087 				fprintf(stderr, "Illegal \"sample\"\n");
1088 				return -1;
1089 			}
1090 			if (sel2.sel.nkeys != 1) {
1091 				fprintf(stderr, "\"sample\" must contain"
1092 					" exactly ONE key.\n");
1093 				return -1;
1094 			}
1095 			if (*argv != 0 && strcmp(*argv, "divisor") == 0) {
1096 				NEXT_ARG();
1097 				if (get_unsigned(&divisor, *argv, 0) || divisor == 0 ||
1098 				    divisor > 0x100 || ((divisor - 1) & divisor)) {
1099 					fprintf(stderr, "Illegal sample \"divisor\"\n");
1100 					return -1;
1101 				}
1102 				NEXT_ARG();
1103 			}
1104 			hash = sel2.sel.keys[0].val & sel2.sel.keys[0].mask;
1105 			hash ^= hash >> 16;
1106 			hash ^= hash >> 8;
1107 			htid = ((hash % divisor) << 12) | (htid & 0xFFF00000);
1108 			sample_ok = 1;
1109 			continue;
1110 		} else if (strcmp(*argv, "indev") == 0) {
1111 			char ind[IFNAMSIZ + 1];
1112 			memset(ind, 0, sizeof (ind));
1113 			argc--;
1114 			argv++;
1115 			if (argc < 1) {
1116 				fprintf(stderr, "Illegal indev\n");
1117 				return -1;
1118 			}
1119 			strncpy(ind, *argv, sizeof (ind) - 1);
1120 			addattr_l(n, MAX_MSG, TCA_U32_INDEV, ind, strlen(ind) + 1);
1121 
1122 		} else if (matches(*argv, "action") == 0) {
1123 			NEXT_ARG();
1124 			if (parse_action(&argc, &argv, TCA_U32_ACT, n)) {
1125 				fprintf(stderr, "Illegal \"action\"\n");
1126 				return -1;
1127 			}
1128 			terminal_ok++;
1129 			continue;
1130 
1131 		} else if (matches(*argv, "police") == 0) {
1132 			NEXT_ARG();
1133 			if (parse_police(&argc, &argv, TCA_U32_POLICE, n)) {
1134 				fprintf(stderr, "Illegal \"police\"\n");
1135 				return -1;
1136 			}
1137 			terminal_ok++;
1138 			continue;
1139 		} else if (strcmp(*argv, "help") == 0) {
1140 			explain();
1141 			return -1;
1142 		} else {
1143 			fprintf(stderr, "What is \"%s\"?\n", *argv);
1144 			explain();
1145 			return -1;
1146 		}
1147 		argc--; argv++;
1148 	}
1149 
1150 	/* We dont necessarily need class/flowids */
1151 	if (terminal_ok)
1152 		sel.sel.flags |= TC_U32_TERMINAL;
1153 
1154 	if (order) {
1155 		if (TC_U32_NODE(t->tcm_handle) && order != TC_U32_NODE(t->tcm_handle)) {
1156 			fprintf(stderr, "\"order\" contradicts \"handle\"\n");
1157 			return -1;
1158 		}
1159 		t->tcm_handle |= order;
1160 	}
1161 
1162 	if (htid)
1163 		addattr_l(n, MAX_MSG, TCA_U32_HASH, &htid, 4);
1164 	if (sel_ok)
1165 		addattr_l(n, MAX_MSG, TCA_U32_SEL, &sel,
1166 			  sizeof(sel.sel) + sel.sel.nkeys * sizeof(struct tc_u32_key));
1167 	tail->rta_len = (void *) NLMSG_TAIL(n) - (void *) tail;
1168 	return 0;
1169 }
1170 
u32_print_opt(struct filter_util * qu,FILE * f,struct rtattr * opt,__u32 handle)1171 static int u32_print_opt(struct filter_util *qu, FILE *f, struct rtattr *opt,
1172 			 __u32 handle)
1173 {
1174 	struct rtattr *tb[TCA_U32_MAX + 1];
1175 	struct tc_u32_sel *sel = NULL;
1176 	struct tc_u32_pcnt *pf = NULL;
1177 
1178 	if (opt == NULL)
1179 		return 0;
1180 
1181 	parse_rtattr_nested(tb, TCA_U32_MAX, opt);
1182 
1183 	if (handle) {
1184 		SPRINT_BUF(b1);
1185 		fprintf(f, "fh %s ", sprint_u32_handle(handle, b1));
1186 	}
1187 	if (TC_U32_NODE(handle)) {
1188 		fprintf(f, "order %d ", TC_U32_NODE(handle));
1189 	}
1190 
1191 	if (tb[TCA_U32_SEL]) {
1192 		if (RTA_PAYLOAD(tb[TCA_U32_SEL])  < sizeof(*sel))
1193 			return -1;
1194 
1195 		sel = RTA_DATA(tb[TCA_U32_SEL]);
1196 	}
1197 
1198 	if (tb[TCA_U32_DIVISOR]) {
1199 		fprintf(f, "ht divisor %d ", rta_getattr_u32(tb[TCA_U32_DIVISOR]));
1200 	} else if (tb[TCA_U32_HASH]) {
1201 		__u32 htid = rta_getattr_u32(tb[TCA_U32_HASH]);
1202 		fprintf(f, "key ht %x bkt %x ", TC_U32_USERHTID(htid),
1203 			TC_U32_HASH(htid));
1204 	} else {
1205 		fprintf(f, "??? ");
1206 	}
1207 	if (tb[TCA_U32_CLASSID]) {
1208 		SPRINT_BUF(b1);
1209 		fprintf(f, "%sflowid %s ",
1210 			!sel || !(sel->flags & TC_U32_TERMINAL) ? "*" : "",
1211 			sprint_tc_classid(rta_getattr_u32(tb[TCA_U32_CLASSID]), b1));
1212 	} else if (sel && sel->flags & TC_U32_TERMINAL) {
1213 		fprintf(f, "terminal flowid ??? ");
1214 	}
1215 	if (tb[TCA_U32_LINK]) {
1216 		SPRINT_BUF(b1);
1217 		fprintf(f, "link %s ",
1218 			sprint_u32_handle(rta_getattr_u32(tb[TCA_U32_LINK]), b1));
1219 	}
1220 
1221 	if (tb[TCA_U32_PCNT]) {
1222 		if (RTA_PAYLOAD(tb[TCA_U32_PCNT])  < sizeof(*pf)) {
1223 			fprintf(f, "Broken perf counters \n");
1224 			return -1;
1225 		}
1226 		pf = RTA_DATA(tb[TCA_U32_PCNT]);
1227 	}
1228 
1229 	if (sel && show_stats && NULL != pf)
1230 		fprintf(f, " (rule hit %llu success %llu)",
1231 			(unsigned long long) pf->rcnt,
1232 			(unsigned long long) pf->rhit);
1233 
1234 	if (tb[TCA_U32_MARK]) {
1235 		struct tc_u32_mark *mark = RTA_DATA(tb[TCA_U32_MARK]);
1236 		if (RTA_PAYLOAD(tb[TCA_U32_MARK]) < sizeof(*mark)) {
1237 			fprintf(f, "\n  Invalid mark (kernel&iproute2 mismatch)\n");
1238 		} else {
1239 			fprintf(f, "\n  mark 0x%04x 0x%04x (success %d)",
1240 				mark->val, mark->mask, mark->success);
1241 		}
1242 	}
1243 
1244 	if (sel) {
1245 		if (sel->nkeys) {
1246 			int i;
1247 			for (i=0; i<sel->nkeys; i++) {
1248 				show_keys(f, sel->keys + i);
1249 				if (show_stats && NULL != pf)
1250 					fprintf(f, " (success %llu ) ",
1251 						(unsigned long long) pf->kcnts[i]);
1252 			}
1253 		}
1254 
1255 		if (sel->flags & (TC_U32_VAROFFSET | TC_U32_OFFSET)) {
1256 			fprintf(f, "\n    offset ");
1257 			if (sel->flags & TC_U32_VAROFFSET)
1258 				fprintf(f, "%04x>>%d at %d ",
1259 					ntohs(sel->offmask),
1260 					sel->offshift,  sel->offoff);
1261 			if (sel->off)
1262 				fprintf(f, "plus %d ", sel->off);
1263 		}
1264 		if (sel->flags & TC_U32_EAT)
1265 			fprintf(f, " eat ");
1266 
1267 		if (sel->hmask) {
1268 			fprintf(f, "\n    hash mask %08x at %d ",
1269 				(unsigned int)htonl(sel->hmask), sel->hoff);
1270 		}
1271 	}
1272 
1273 	if (tb[TCA_U32_POLICE]) {
1274 		fprintf(f, "\n");
1275 		tc_print_police(f, tb[TCA_U32_POLICE]);
1276 	}
1277 	if (tb[TCA_U32_INDEV]) {
1278 		struct rtattr *idev = tb[TCA_U32_INDEV];
1279 		fprintf(f, "\n  input dev %s\n", rta_getattr_str(idev));
1280 	}
1281 	if (tb[TCA_U32_ACT]) {
1282 		tc_print_action(f, tb[TCA_U32_ACT]);
1283 	}
1284 
1285 	return 0;
1286 }
1287 
1288 struct filter_util u32_filter_util = {
1289 	.id = "u32",
1290 	.parse_fopt = u32_parse_opt,
1291 	.print_fopt = u32_print_opt,
1292 };
1293