f_rsvp.c revision e9acc2420c561a6f875d188de1028facbd09c5a8 
1/*
2 * q_rsvp.c		RSVP filter.
3 *
4 *		This program is free software; you can redistribute 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 *
11 */
12
13#include <stdio.h>
14#include <stdlib.h>
15#include <unistd.h>
16#include <syslog.h>
17#include <fcntl.h>
18#include <sys/socket.h>
19#include <netinet/in.h>
20#include <arpa/inet.h>
21#include <string.h>
22
23#include "rt_names.h"
24#include "utils.h"
25#include "tc_util.h"
26
27static void explain(void)
28{
29	fprintf(stderr, "Usage: ... rsvp ipproto PROTOCOL session DST[/PORT | GPI ]\n");
30	fprintf(stderr, "                [ sender SRC[/PORT | GPI ]\n");
31	fprintf(stderr, "                [ classid CLASSID ] [ police POLICE_SPEC ]\n");
32	fprintf(stderr, "                [ tunnelid ID ] [ tunnel ID skip NUMBER ]\n");
33	fprintf(stderr, "Where: GPI := { flowlabel NUMBER | spi/ah SPI | spi/esp SPI |\n");
34	fprintf(stderr, "                u{8|16|32} NUMBER mask MASK at OFFSET}\n");
35	fprintf(stderr, "       POLICE_SPEC := ... look at TBF\n");
36	fprintf(stderr, "       FILTERID := X:Y\n");
37	fprintf(stderr, "\nNOTE: CLASSID is parsed as hexadecimal input.\n");
38}
39
40#define usage() return(-1)
41
42int get_addr_and_pi(int *argc_p, char ***argv_p, inet_prefix * addr,
43		    struct tc_rsvp_pinfo *pinfo, int dir, int family)
44{
45	int argc = *argc_p;
46	char **argv = *argv_p;
47	char *p = strchr(*argv, '/');
48	struct tc_rsvp_gpi *pi = dir ? &pinfo->dpi : &pinfo->spi;
49
50	if (p) {
51		__u16 tmp;
52
53		if (get_u16(&tmp, p+1, 0))
54			return -1;
55
56		if (dir == 0) {
57			/* Source port: u16 at offset 0 */
58			pi->key = htonl(((__u32)tmp)<<16);
59			pi->mask = htonl(0xFFFF0000);
60		} else {
61			/* Destination port: u16 at offset 2 */
62			pi->key = htonl(((__u32)tmp));
63			pi->mask = htonl(0x0000FFFF);
64		}
65		pi->offset = 0;
66		*p = 0;
67	}
68	if (get_addr_1(addr, *argv, family))
69		return -1;
70	if (p)
71		*p = '/';
72
73	argc--; argv++;
74
75	if (pi->mask || argc <= 0)
76		goto done;
77
78	if (strcmp(*argv, "spi/ah") == 0 ||
79	    strcmp(*argv, "gpi/ah") == 0) {
80		__u32 gpi;
81		NEXT_ARG();
82		if (get_u32(&gpi, *argv, 0))
83			return -1;
84		pi->mask = htonl(0xFFFFFFFF);
85		pi->key = htonl(gpi);
86		pi->offset = 4;
87		if (pinfo->protocol == 0)
88			pinfo->protocol = IPPROTO_AH;
89		argc--; argv++;
90	} else if (strcmp(*argv, "spi/esp") == 0 ||
91		   strcmp(*argv, "gpi/esp") == 0) {
92		__u32 gpi;
93		NEXT_ARG();
94		if (get_u32(&gpi, *argv, 0))
95			return -1;
96		pi->mask = htonl(0xFFFFFFFF);
97		pi->key = htonl(gpi);
98		pi->offset = 0;
99		if (pinfo->protocol == 0)
100			pinfo->protocol = IPPROTO_ESP;
101		argc--; argv++;
102	} else if (strcmp(*argv, "flowlabel") == 0) {
103		__u32 flabel;
104		NEXT_ARG();
105		if (get_u32(&flabel, *argv, 0))
106			return -1;
107		if (family != AF_INET6)
108			return -1;
109		pi->mask = htonl(0x000FFFFF);
110		pi->key = htonl(flabel) & pi->mask;
111		pi->offset = -40;
112		argc--; argv++;
113	} else if (strcmp(*argv, "u32") == 0 ||
114		   strcmp(*argv, "u16") == 0 ||
115		   strcmp(*argv, "u8") == 0) {
116		int sz = 1;
117		__u32 tmp;
118		__u32 mask = 0xff;
119		if (strcmp(*argv, "u32") == 0) {
120			sz = 4;
121			mask = 0xffff;
122		} else if (strcmp(*argv, "u16") == 0) {
123			mask = 0xffffffff;
124			sz = 2;
125		}
126		NEXT_ARG();
127		if (get_u32(&tmp, *argv, 0))
128			return -1;
129		argc--; argv++;
130		if (strcmp(*argv, "mask") == 0) {
131			NEXT_ARG();
132			if (get_u32(&mask, *argv, 16))
133				return -1;
134			argc--; argv++;
135		}
136		if (strcmp(*argv, "at") == 0) {
137			NEXT_ARG();
138			if (get_integer(&pi->offset, *argv, 0))
139				return -1;
140			argc--; argv++;
141		}
142		if (sz == 1) {
143			if ((pi->offset & 3) == 0) {
144				mask <<= 24;
145				tmp <<= 24;
146			} else if ((pi->offset & 3) == 1) {
147				mask <<= 16;
148				tmp <<= 16;
149			} else if ((pi->offset & 3) == 3) {
150				mask <<= 8;
151				tmp <<= 8;
152			}
153		} else if (sz == 2) {
154			if ((pi->offset & 3) == 0) {
155				mask <<= 16;
156				tmp <<= 16;
157			}
158		}
159		pi->offset &= ~3;
160		pi->mask = htonl(mask);
161		pi->key = htonl(tmp) & pi->mask;
162	}
163
164done:
165	*argc_p = argc;
166	*argv_p = argv;
167	return 0;
168}
169
170
171static int rsvp_parse_opt(struct filter_util *qu, char *handle, int argc, char **argv, struct nlmsghdr *n)
172{
173	int family = strcmp(qu->id, "rsvp") == 0 ? AF_INET : AF_INET6;
174	struct tc_rsvp_pinfo pinfo;
175	struct tc_police tp;
176	struct tcmsg *t = NLMSG_DATA(n);
177	int pinfo_ok = 0;
178	struct rtattr *tail;
179
180	memset(&pinfo, 0, sizeof(pinfo));
181	memset(&tp, 0, sizeof(tp));
182
183	if (handle) {
184		if (get_u32(&t->tcm_handle, handle, 0)) {
185			fprintf(stderr, "Illegal \"handle\"\n");
186			return -1;
187		}
188	}
189
190	if (argc == 0)
191		return 0;
192
193	tail = NLMSG_TAIL(n);
194	addattr_l(n, 4096, TCA_OPTIONS, NULL, 0);
195
196	while (argc > 0) {
197		if (matches(*argv, "session") == 0) {
198			inet_prefix addr;
199			NEXT_ARG();
200			if (get_addr_and_pi(&argc, &argv, &addr, &pinfo, 1, family)) {
201				fprintf(stderr, "Illegal \"session\"\n");
202				return -1;
203			}
204			addattr_l(n, 4096, TCA_RSVP_DST, &addr.data, addr.bytelen);
205			if (pinfo.dpi.mask || pinfo.protocol)
206				pinfo_ok++;
207			continue;
208		} else if (matches(*argv, "sender") == 0 ||
209			   matches(*argv, "flowspec") == 0) {
210			inet_prefix addr;
211			NEXT_ARG();
212			if (get_addr_and_pi(&argc, &argv, &addr, &pinfo, 0, family)) {
213				fprintf(stderr, "Illegal \"sender\"\n");
214				return -1;
215			}
216			addattr_l(n, 4096, TCA_RSVP_SRC, &addr.data, addr.bytelen);
217			if (pinfo.spi.mask || pinfo.protocol)
218				pinfo_ok++;
219			continue;
220		} else if (matches("ipproto", *argv) == 0) {
221			int num;
222			NEXT_ARG();
223			num = inet_proto_a2n(*argv);
224			if (num < 0) {
225				fprintf(stderr, "Illegal \"ipproto\"\n");
226				return -1;
227			}
228			pinfo.protocol = num;
229			pinfo_ok++;
230		} else if (matches(*argv, "classid") == 0 ||
231			   strcmp(*argv, "flowid") == 0) {
232			unsigned handle;
233			NEXT_ARG();
234			if (get_tc_classid(&handle, *argv)) {
235				fprintf(stderr, "Illegal \"classid\"\n");
236				return -1;
237			}
238			addattr_l(n, 4096, TCA_RSVP_CLASSID, &handle, 4);
239		} else if (strcmp(*argv, "tunnelid") == 0) {
240			unsigned tid;
241			NEXT_ARG();
242			if (get_unsigned(&tid, *argv, 0)) {
243				fprintf(stderr, "Illegal \"tunnelid\"\n");
244				return -1;
245			}
246			pinfo.tunnelid = tid;
247			pinfo_ok++;
248		} else if (strcmp(*argv, "tunnel") == 0) {
249			unsigned tid;
250			NEXT_ARG();
251			if (get_unsigned(&tid, *argv, 0)) {
252				fprintf(stderr, "Illegal \"tunnel\"\n");
253				return -1;
254			}
255			addattr_l(n, 4096, TCA_RSVP_CLASSID, &tid, 4);
256			NEXT_ARG();
257			if (strcmp(*argv, "skip") == 0) {
258				NEXT_ARG();
259			}
260			if (get_unsigned(&tid, *argv, 0)) {
261				fprintf(stderr, "Illegal \"skip\"\n");
262				return -1;
263			}
264			pinfo.tunnelhdr = tid;
265			pinfo_ok++;
266		} else if (matches(*argv, "police") == 0) {
267			NEXT_ARG();
268			if (parse_police(&argc, &argv, TCA_RSVP_POLICE, n)) {
269				fprintf(stderr, "Illegal \"police\"\n");
270				return -1;
271			}
272			continue;
273		} else if (strcmp(*argv, "help") == 0) {
274			explain();
275			return -1;
276		} else {
277			fprintf(stderr, "What is \"%s\"?\n", *argv);
278			explain();
279			return -1;
280		}
281		argc--; argv++;
282	}
283
284	if (pinfo_ok)
285		addattr_l(n, 4096, TCA_RSVP_PINFO, &pinfo, sizeof(pinfo));
286	tail->rta_len = (void *) NLMSG_TAIL(n) - (void *) tail;
287	return 0;
288}
289
290static char * sprint_spi(struct tc_rsvp_gpi *pi, int dir, char *buf)
291{
292	if (pi->offset == 0) {
293		if (dir && pi->mask == htonl(0xFFFF)) {
294			snprintf(buf, SPRINT_BSIZE-1, "/%d", htonl(pi->key));
295			return buf;
296		}
297		if (!dir && pi->mask == htonl(0xFFFF0000)) {
298			snprintf(buf, SPRINT_BSIZE-1, "/%d", htonl(pi->key)>>16);
299			return buf;
300		}
301		if (pi->mask == htonl(0xFFFFFFFF)) {
302			snprintf(buf, SPRINT_BSIZE-1, " spi/esp 0x%08x", htonl(pi->key));
303			return buf;
304		}
305	} else if (pi->offset == 4 && pi->mask == htonl(0xFFFFFFFF)) {
306		snprintf(buf, SPRINT_BSIZE-1, " spi/ah 0x%08x", htonl(pi->key));
307		return buf;
308	} else if (pi->offset == -40 && pi->mask == htonl(0x000FFFFF)) {
309		snprintf(buf, SPRINT_BSIZE-1, " flowlabel 0x%05x", htonl(pi->key));
310		return buf;
311	}
312	snprintf(buf, SPRINT_BSIZE-1, " u32 0x%08x mask %08x at %d",
313		 htonl(pi->key), htonl(pi->mask), pi->offset);
314	return buf;
315}
316
317static int rsvp_print_opt(struct filter_util *qu, FILE *f, struct rtattr *opt, __u32 handle)
318{
319	int family = strcmp(qu->id, "rsvp") == 0 ? AF_INET : AF_INET6;
320	struct rtattr *tb[TCA_RSVP_MAX+1];
321	struct tc_rsvp_pinfo *pinfo = NULL;
322
323	if (opt == NULL)
324		return 0;
325
326	parse_rtattr_nested(tb, TCA_RSVP_MAX, opt);
327
328	if (handle)
329		fprintf(f, "fh 0x%08x ", handle);
330
331	if (tb[TCA_RSVP_PINFO]) {
332		if (RTA_PAYLOAD(tb[TCA_RSVP_PINFO])  < sizeof(*pinfo))
333			return -1;
334
335		pinfo = RTA_DATA(tb[TCA_RSVP_PINFO]);
336	}
337
338	if (tb[TCA_RSVP_CLASSID]) {
339		SPRINT_BUF(b1);
340		if (!pinfo || pinfo->tunnelhdr == 0)
341			fprintf(f, "flowid %s ", sprint_tc_classid(*(__u32*)RTA_DATA(tb[TCA_RSVP_CLASSID]), b1));
342		else
343			fprintf(f, "tunnel %d skip %d ", *(__u32*)RTA_DATA(tb[TCA_RSVP_CLASSID]), pinfo->tunnelhdr);
344	} else if (pinfo && pinfo->tunnelhdr)
345		fprintf(f, "tunnel [BAD] skip %d ", pinfo->tunnelhdr);
346
347	if (tb[TCA_RSVP_DST]) {
348		char buf[128];
349		fprintf(f, "session ");
350		if (inet_ntop(family, RTA_DATA(tb[TCA_RSVP_DST]), buf, sizeof(buf)) == 0)
351			fprintf(f, " [INVALID DADDR] ");
352		else
353			fprintf(f, "%s", buf);
354		if (pinfo && pinfo->dpi.mask) {
355			SPRINT_BUF(b2);
356			fprintf(f, "%s ", sprint_spi(&pinfo->dpi, 1, b2));
357		} else
358			fprintf(f, " ");
359	} else {
360		if (pinfo && pinfo->dpi.mask) {
361			SPRINT_BUF(b2);
362			fprintf(f, "session [NONE]%s ", sprint_spi(&pinfo->dpi, 1, b2));
363		} else
364			fprintf(f, "session NONE ");
365	}
366
367	if (pinfo && pinfo->protocol) {
368		SPRINT_BUF(b1);
369		fprintf(f, "ipproto %s ", inet_proto_n2a(pinfo->protocol, b1, sizeof(b1)));
370	}
371	if (pinfo && pinfo->tunnelid)
372		fprintf(f, "tunnelid %d ", pinfo->tunnelid);
373	if (tb[TCA_RSVP_SRC]) {
374		char buf[128];
375		fprintf(f, "sender ");
376		if (inet_ntop(family, RTA_DATA(tb[TCA_RSVP_SRC]), buf, sizeof(buf)) == 0) {
377			fprintf(f, "[BAD]");
378		} else {
379			fprintf(f, " %s", buf);
380		}
381		if (pinfo && pinfo->spi.mask) {
382			SPRINT_BUF(b2);
383			fprintf(f, "%s ", sprint_spi(&pinfo->spi, 0, b2));
384		} else
385			fprintf(f, " ");
386	} else if (pinfo && pinfo->spi.mask) {
387		SPRINT_BUF(b2);
388		fprintf(f, "sender [NONE]%s ", sprint_spi(&pinfo->spi, 0, b2));
389	}
390	if (tb[TCA_RSVP_POLICE])
391		tc_print_police(f, tb[TCA_RSVP_POLICE]);
392	return 0;
393}
394
395struct filter_util rsvp_filter_util = {
396	.id = "rsvp",
397	.parse_fopt = rsvp_parse_opt,
398	.print_fopt = rsvp_print_opt,
399};
400
401struct filter_util rsvp6_filter_util = {
402	.id = "rsvp6",
403	.parse_fopt = rsvp_parse_opt,
404	.print_fopt = rsvp_print_opt,
405};
406