1/*
2 * mapcalc - MAP parameter calculation
3 *
4 * Author: Steven Barth <cyrus@openwrt.org>
5 * Copyright (c) 2014-2015 cisco Systems, Inc.
6 * Copyright (c) 2015 Steven Barth <cyrus@openwrt.org>
7 *
8 * This program is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License version 2
10 * as published by the Free Software Foundation
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15 * GNU General Public License for more details.
16 */
17
18#include <stdlib.h>
19#include <stdio.h>
20#include <arpa/inet.h>
21#include <errno.h>
22#include <libubus.h>
23#include <libubox/utils.h>
24
25
26struct blob_attr *dump = NULL;
27
28enum {
29	DUMP_ATTR_INTERFACE,
30	DUMP_ATTR_MAX
31};
32
33static const struct blobmsg_policy dump_attrs[DUMP_ATTR_MAX] = {
34	[DUMP_ATTR_INTERFACE] = { .name = "interface", .type = BLOBMSG_TYPE_ARRAY },
35};
36
37
38enum {
39	IFACE_ATTR_INTERFACE,
40	IFACE_ATTR_PREFIX,
41	IFACE_ATTR_ADDRESS,
42	IFACE_ATTR_MAX,
43};
44
45static const struct blobmsg_policy iface_attrs[IFACE_ATTR_MAX] = {
46	[IFACE_ATTR_INTERFACE] = { .name = "interface", .type = BLOBMSG_TYPE_STRING },
47	[IFACE_ATTR_PREFIX] = { .name = "ipv6-prefix", .type = BLOBMSG_TYPE_ARRAY },
48	[IFACE_ATTR_ADDRESS] = { .name = "ipv6-address", .type = BLOBMSG_TYPE_ARRAY },
49};
50
51
52enum {
53	PREFIX_ATTR_ADDRESS,
54	PREFIX_ATTR_MASK,
55	PREFIX_ATTR_MAX,
56};
57
58static const struct blobmsg_policy prefix_attrs[PREFIX_ATTR_MAX] = {
59	[PREFIX_ATTR_ADDRESS] = { .name = "address", .type = BLOBMSG_TYPE_STRING },
60	[PREFIX_ATTR_MASK] = { .name = "mask", .type = BLOBMSG_TYPE_INT32 },
61};
62
63static int bmemcmp(const void *av, const void *bv, size_t bits)
64{
65	const uint8_t *a = av, *b = bv;
66	size_t bytes = bits / 8;
67	bits %= 8;
68
69	int res = memcmp(a, b, bytes);
70	if (res == 0 && bits > 0)
71		res = (a[bytes] >> (8 - bits)) - (b[bytes] >> (8 - bits));
72
73	return res;
74}
75
76static void bmemcpy(void *av, const void *bv, size_t bits)
77{
78	uint8_t *a = av;
79	const uint8_t *b = bv;
80
81	size_t bytes = bits / 8;
82	bits %= 8;
83	memcpy(a, b, bytes);
84
85	if (bits > 0) {
86		uint8_t mask = (1 << (8 - bits)) - 1;
87		a[bytes] = (a[bytes] & mask) | ((~mask) & b[bytes]);
88	}
89}
90
91static void bmemcpys64(void *av, const void *bv, size_t frombits, size_t nbits)
92{
93	uint64_t buf = 0;
94	const uint8_t *b = bv;
95	size_t frombyte = frombits / 8, tobyte = (frombits + nbits) / 8;
96
97	memcpy(&buf, &b[frombyte], tobyte - frombyte + 1);
98	buf = cpu_to_be64(be64_to_cpu(buf) << (frombits % 8));
99
100	bmemcpy(av, &buf, nbits);
101}
102
103static void handle_dump(struct ubus_request *req __attribute__((unused)),
104		int type __attribute__((unused)), struct blob_attr *msg)
105{
106	struct blob_attr *tb[DUMP_ATTR_INTERFACE];
107	blobmsg_parse(dump_attrs, DUMP_ATTR_MAX, tb, blob_data(msg), blob_len(msg));
108
109	if (!tb[DUMP_ATTR_INTERFACE])
110		return;
111
112	dump = blob_memdup(tb[DUMP_ATTR_INTERFACE]);
113}
114
115static void match_prefix(int *pdlen, struct in6_addr *pd, struct blob_attr *cur,
116		const struct in6_addr *ipv6prefix, int prefix6len, bool lw4o6)
117{
118	struct blob_attr *d;
119	unsigned drem;
120
121	if (!cur || blobmsg_type(cur) != BLOBMSG_TYPE_ARRAY || !blobmsg_check_attr(cur, NULL))
122		return;
123
124	blobmsg_for_each_attr(d, cur, drem) {
125		struct blob_attr *ptb[PREFIX_ATTR_MAX];
126		blobmsg_parse(prefix_attrs, PREFIX_ATTR_MAX, ptb,
127				blobmsg_data(d), blobmsg_data_len(d));
128
129		if (!ptb[PREFIX_ATTR_ADDRESS] || !ptb[PREFIX_ATTR_MASK])
130			continue;
131
132		struct in6_addr prefix = IN6ADDR_ANY_INIT;
133		int mask = blobmsg_get_u32(ptb[PREFIX_ATTR_MASK]);
134		inet_pton(AF_INET6, blobmsg_get_string(ptb[PREFIX_ATTR_ADDRESS]), &prefix);
135
136		// lw4over6 /128-address-as-PD matching madness workaround
137		if (lw4o6 && mask == 128)
138			mask = 64;
139
140		if (*pdlen < mask && mask >= prefix6len &&
141				!bmemcmp(&prefix, ipv6prefix, prefix6len)) {
142			bmemcpy(pd, &prefix, mask);
143			*pdlen = mask;
144		} else if (lw4o6 && *pdlen < prefix6len && mask < prefix6len &&
145				!bmemcmp(&prefix, ipv6prefix, mask)) {
146			bmemcpy(pd, ipv6prefix, prefix6len);
147			*pdlen = prefix6len;
148		}
149	}
150}
151
152enum {
153	OPT_TYPE,
154	OPT_FMR,
155	OPT_EALEN,
156	OPT_PREFIX4LEN,
157	OPT_PREFIX6LEN,
158	OPT_IPV6PREFIX,
159	OPT_IPV4PREFIX,
160	OPT_OFFSET,
161	OPT_PSIDLEN,
162	OPT_PSID,
163	OPT_BR,
164	OPT_DMR,
165	OPT_PD,
166	OPT_PDLEN,
167	OPT_MAX
168};
169
170static char *const token[] = {
171	[OPT_TYPE] = "type",
172	[OPT_FMR] = "fmr",
173	[OPT_EALEN] = "ealen",
174	[OPT_PREFIX4LEN] = "prefix4len",
175	[OPT_PREFIX6LEN] = "prefix6len",
176	[OPT_IPV6PREFIX] = "ipv6prefix",
177	[OPT_IPV4PREFIX] = "ipv4prefix",
178	[OPT_OFFSET] = "offset",
179	[OPT_PSIDLEN] = "psidlen",
180	[OPT_PSID] = "psid",
181	[OPT_BR] = "br",
182	[OPT_DMR] = "dmr",
183	[OPT_PD] = "pd",
184	[OPT_PDLEN] = "pdlen",
185	[OPT_MAX] = NULL
186};
187
188
189int main(int argc, char *argv[])
190{
191	int status = 0;
192	const char *iface = argv[1];
193
194	const char *legacy_env = getenv("LEGACY");
195	bool legacy = legacy_env && atoi(legacy_env);
196
197
198	if (argc < 3) {
199		fprintf(stderr, "Usage: %s <interface|*> <rule1> [rule2] [...]\n", argv[0]);
200		return 1;
201	}
202
203	uint32_t network_interface;
204	struct ubus_context *ubus = ubus_connect(NULL);
205	if (ubus) {
206		ubus_lookup_id(ubus, "network.interface", &network_interface);
207		ubus_invoke(ubus, network_interface, "dump", NULL, handle_dump, NULL, 5000);
208	}
209
210	int rulecnt = 0;
211	for (int i = 2; i < argc; ++i) {
212		bool lw4o6 = false;
213		bool fmr = false;
214		int ealen = -1;
215		int addr4len = 32;
216		int prefix4len = 32;
217		int prefix6len = -1;
218		int pdlen = -1;
219		struct in_addr ipv4prefix = {INADDR_ANY};
220		struct in_addr ipv4addr = {INADDR_ANY};
221		struct in6_addr ipv6addr = IN6ADDR_ANY_INIT;
222		struct in6_addr ipv6prefix = IN6ADDR_ANY_INIT;
223		struct in6_addr pd = IN6ADDR_ANY_INIT;
224		int offset = -1;
225		int psidlen = -1;
226		int psid = -1;
227		uint16_t psid16 = 0;
228		const char *dmr = NULL;
229		const char *br = NULL;
230
231		for (char *rule = strdup(argv[i]); *rule; ) {
232			char *value;
233			int intval;
234			int idx = getsubopt(&rule, token, &value);
235			errno = 0;
236
237			if (idx == OPT_TYPE) {
238				lw4o6 = (value && !strcmp(value, "lw4o6"));
239			} else if (idx == OPT_FMR) {
240				fmr = true;
241			} else if (idx == OPT_EALEN && (intval = strtoul(value, NULL, 0)) <= 48 && !errno) {
242				ealen = intval;
243			} else if (idx == OPT_PREFIX4LEN && (intval = strtoul(value, NULL, 0)) <= 32 && !errno) {
244				prefix4len = intval;
245			} else if (idx == OPT_PREFIX6LEN && (intval = strtoul(value, NULL, 0)) <= 128 && !errno) {
246				prefix6len = intval;
247			} else if (idx == OPT_IPV4PREFIX && inet_pton(AF_INET, value, &ipv4prefix) == 1) {
248				// dummy
249			} else if (idx == OPT_IPV6PREFIX && inet_pton(AF_INET6, value, &ipv6prefix) == 1) {
250				// dummy
251			} else if (idx == OPT_PD && inet_pton(AF_INET6, value, &pd) == 1) {
252				// dummy
253			} else if (idx == OPT_OFFSET && (intval = strtoul(value, NULL, 0)) <= 16 && !errno) {
254				offset = intval;
255			} else if (idx == OPT_PSIDLEN && (intval = strtoul(value, NULL, 0)) <= 16 && !errno) {
256				psidlen = intval;
257			} else if (idx == OPT_PDLEN && (intval = strtoul(value, NULL, 0)) <= 128 && !errno) {
258				pdlen = intval;
259			} else if (idx == OPT_PSID && (intval = strtoul(value, NULL, 0)) <= 65535 && !errno) {
260				psid = intval;
261			} else if (idx == OPT_DMR) {
262				dmr = value;
263			} else if (idx == OPT_BR) {
264				br = value;
265			} else {
266				if (idx == -1 || idx >= OPT_MAX)
267					fprintf(stderr, "Skipped invalid option: %s\n", value);
268				else
269					fprintf(stderr, "Skipped invalid value %s for option %s\n",
270							value, token[idx]);
271			}
272		}
273
274		if (offset < 0)
275			offset = (lw4o6) ? 0 : (legacy) ? 4 : 6;
276
277		// LW4over6 doesn't have an EALEN and has no psid-autodetect
278		if (lw4o6) {
279			if (psidlen < 0)
280				psidlen = 0;
281
282			ealen = psidlen;
283		}
284
285		// Find PD
286		if (pdlen < 0) {
287			struct blob_attr *c;
288			unsigned rem;
289			blobmsg_for_each_attr(c, dump, rem) {
290				struct blob_attr *tb[IFACE_ATTR_MAX];
291				blobmsg_parse(iface_attrs, IFACE_ATTR_MAX, tb, blobmsg_data(c), blobmsg_data_len(c));
292
293				if (!tb[IFACE_ATTR_INTERFACE] || (strcmp(argv[1], "*") && strcmp(argv[1],
294						blobmsg_get_string(tb[IFACE_ATTR_INTERFACE]))))
295					continue;
296
297				match_prefix(&pdlen, &pd, tb[IFACE_ATTR_PREFIX], &ipv6prefix, prefix6len, lw4o6);
298
299				if (lw4o6)
300					match_prefix(&pdlen, &pd, tb[IFACE_ATTR_ADDRESS], &ipv6prefix, prefix6len, lw4o6);
301
302				if (pdlen >= 0) {
303					iface = blobmsg_get_string(tb[IFACE_ATTR_INTERFACE]);
304					break;
305				}
306			}
307		}
308
309		if (ealen < 0 && pdlen >= 0)
310			ealen = pdlen - prefix6len;
311
312		if (psidlen <= 0) {
313			psidlen = ealen - (32 - prefix4len);
314			psid = -1;
315		}
316
317		if (psid < 0 && psidlen <= 16 && psidlen >= 0 && pdlen >= 0 && ealen >= psidlen) {
318			bmemcpys64(&psid16, &pd, prefix6len + ealen - psidlen, psidlen);
319			psid = be16_to_cpu(psid16);
320		}
321
322		psid = psid >> (16 - psidlen);
323		psid16 = cpu_to_be16(psid);
324		psid = psid << (16 - psidlen);
325
326		if (prefix4len < 0 || prefix6len < 0 || ealen < 0 || ealen < psidlen) {
327			fprintf(stderr, "Skipping invalid or incomplete rule: %s\n", argv[i]);
328			status = 1;
329			continue;
330		}
331
332		if ((pdlen >= 0 || ealen == psidlen) && ealen >= psidlen) {
333			bmemcpys64(&ipv4addr, &pd, prefix6len, ealen - psidlen);
334			ipv4addr.s_addr = htonl(ntohl(ipv4addr.s_addr) >> prefix4len);
335			bmemcpy(&ipv4addr, &ipv4prefix, prefix4len);
336
337			if (prefix4len + ealen < 32)
338				addr4len = prefix4len + ealen;
339		}
340
341		if (pdlen < 0 && !fmr) {
342			fprintf(stderr, "Skipping non-FMR without matching PD: %s\n", argv[i]);
343			status = 1;
344			continue;
345		} else if (pdlen >= 0) {
346			size_t v4offset = (legacy) ? 9 : 10;
347			memcpy(&ipv6addr.s6_addr[v4offset], &ipv4addr, 4);
348			memcpy(&ipv6addr.s6_addr[v4offset + 4], &psid16, 2);
349			bmemcpy(&ipv6addr, &pd, pdlen);
350		}
351
352		++rulecnt;
353		char ipv4addrbuf[INET_ADDRSTRLEN];
354		char ipv4prefixbuf[INET_ADDRSTRLEN];
355		char ipv6prefixbuf[INET6_ADDRSTRLEN];
356		char ipv6addrbuf[INET6_ADDRSTRLEN];
357		char pdbuf[INET6_ADDRSTRLEN];
358
359		inet_ntop(AF_INET, &ipv4addr, ipv4addrbuf, sizeof(ipv4addrbuf));
360		inet_ntop(AF_INET, &ipv4prefix, ipv4prefixbuf, sizeof(ipv4prefixbuf));
361		inet_ntop(AF_INET6, &ipv6prefix, ipv6prefixbuf, sizeof(ipv6prefixbuf));
362		inet_ntop(AF_INET6, &ipv6addr, ipv6addrbuf, sizeof(ipv6addrbuf));
363		inet_ntop(AF_INET6, &pd, pdbuf, sizeof(pdbuf));
364
365		printf("RULE_%d_FMR=%d\n", rulecnt, fmr);
366		printf("RULE_%d_EALEN=%d\n", rulecnt, ealen);
367		printf("RULE_%d_PSIDLEN=%d\n", rulecnt, psidlen);
368		printf("RULE_%d_OFFSET=%d\n", rulecnt, offset);
369		printf("RULE_%d_PREFIX4LEN=%d\n", rulecnt, prefix4len);
370		printf("RULE_%d_PREFIX6LEN=%d\n", rulecnt, prefix6len);
371		printf("RULE_%d_IPV4PREFIX=%s\n", rulecnt, ipv4prefixbuf);
372		printf("RULE_%d_IPV6PREFIX=%s\n", rulecnt, ipv6prefixbuf);
373
374		if (pdlen >= 0) {
375			printf("RULE_%d_IPV6PD=%s\n", rulecnt, pdbuf);
376			printf("RULE_%d_PD6LEN=%d\n", rulecnt, pdlen);
377			printf("RULE_%d_PD6IFACE=%s\n", rulecnt, iface);
378			printf("RULE_%d_IPV6ADDR=%s\n", rulecnt, ipv6addrbuf);
379			printf("RULE_BMR=%d\n", rulecnt);
380		}
381
382		if (ipv4addr.s_addr) {
383			printf("RULE_%d_IPV4ADDR=%s\n", rulecnt, ipv4addrbuf);
384			printf("RULE_%d_ADDR4LEN=%d\n", rulecnt, addr4len);
385		}
386
387
388		if (psidlen > 0 && psid >= 0) {
389			printf("RULE_%d_PORTSETS='", rulecnt);
390			for (int k = (offset) ? 1 : 0; k < (1 << offset); ++k) {
391				int start = (k << (16 - offset)) | (psid >> offset);
392				int end = start + (1 << (16 - offset - psidlen)) - 1;
393
394				if (start == 0)
395					start = 1;
396
397				if (start <= end)
398					printf("%d-%d ", start, end);
399			}
400			printf("'\n");
401		}
402
403		if (dmr)
404			printf("RULE_%d_DMR=%s\n", rulecnt, dmr);
405
406		if (br)
407			printf("RULE_%d_BR=%s\n", rulecnt, br);
408	}
409
410	printf("RULE_COUNT=%d\n", rulecnt);
411	return status;
412}
413