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 }