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