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