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