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