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  *
   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                         psid = -1;
 277                 }
 278
 279                 if (psid < 0 && psidlen <= 16 && psidlen >= 0 && pdlen >= 0 && ealen >= psidlen) {
 280                         bmemcpys64(&psid16, &pd, prefix6len + ealen - psidlen, psidlen);
 281                         psid = be16_to_cpu(psid16);
 282                 }
 283
 284                 psid16 = cpu_to_be16(psid >> (16 - psidlen));
 285
 286                 if ((pdlen >= 0 || ealen == psidlen) && ealen >= psidlen) {
 287                         bmemcpys64(&ipv4addr, &pd, prefix6len, ealen - psidlen);
 288                         ipv4addr.s_addr = htonl(ntohl(ipv4addr.s_addr) >> prefix4len);
 289                         bmemcpy(&ipv4addr, &ipv4prefix, prefix4len);
 290
 291                         if (prefix4len + ealen < 32)
 292                                 addr4len = prefix4len + ealen;
 293                 }
 294
 295                 if (prefix4len < 0 || prefix6len < 0 || ealen < 0 || ealen < psidlen) {
 296                         fprintf(stderr, "Skipping invalid or incomplete rule: %s\n", argv[i]);
 297                         status = 1;
 298                         continue;
 299                 }
 300
 301                 if (pdlen < 0 && !fmr) {
 302                         fprintf(stderr, "Skipping non-FMR without matching PD: %s\n", argv[i]);
 303                         status = 1;
 304                         continue;
 305                 } else if (pdlen >= 0) {
 306                         size_t v4offset = (legacy) ? 9 : 10;
 307                         memcpy(&ipv6addr.s6_addr[v4offset], &ipv4addr, 4);
 308                         memcpy(&ipv6addr.s6_addr[v4offset + 4], &psid16, 2);
 309                         bmemcpy(&ipv6addr, &pd, pdlen);
 310                 }
 311
 312                 ++rulecnt;
 313                 char ipv4addrbuf[INET_ADDRSTRLEN];
 314                 char ipv4prefixbuf[INET_ADDRSTRLEN];
 315                 char ipv6prefixbuf[INET6_ADDRSTRLEN];
 316                 char ipv6addrbuf[INET6_ADDRSTRLEN];
 317                 char pdbuf[INET6_ADDRSTRLEN];
 318
 319                 inet_ntop(AF_INET, &ipv4addr, ipv4addrbuf, sizeof(ipv4addrbuf));
 320                 inet_ntop(AF_INET, &ipv4prefix, ipv4prefixbuf, sizeof(ipv4prefixbuf));
 321                 inet_ntop(AF_INET6, &ipv6prefix, ipv6prefixbuf, sizeof(ipv6prefixbuf));
 322                 inet_ntop(AF_INET6, &ipv6addr, ipv6addrbuf, sizeof(ipv6addrbuf));
 323                 inet_ntop(AF_INET6, &pd, pdbuf, sizeof(pdbuf));
 324
 325                 printf("RULE_%d_FMR=%d\n", rulecnt, fmr);
 326                 printf("RULE_%d_EALEN=%d\n", rulecnt, ealen);
 327                 printf("RULE_%d_PSIDLEN=%d\n", rulecnt, psidlen);
 328                 printf("RULE_%d_OFFSET=%d\n", rulecnt, offset);
 329                 printf("RULE_%d_PREFIX4LEN=%d\n", rulecnt, prefix4len);
 330                 printf("RULE_%d_PREFIX6LEN=%d\n", rulecnt, prefix6len);
 331                 printf("RULE_%d_IPV4PREFIX=%s\n", rulecnt, ipv4prefixbuf);
 332                 printf("RULE_%d_IPV6PREFIX=%s\n", rulecnt, ipv6prefixbuf);
 333
 334                 if (pdlen >= 0) {
 335                         printf("RULE_%d_IPV6PD=%s\n", rulecnt, pdbuf);
 336                         printf("RULE_%d_PD6LEN=%d\n", rulecnt, pdlen);
 337                         printf("RULE_%d_PD6IFACE=%s\n", rulecnt, iface);
 338                         printf("RULE_%d_IPV6ADDR=%s\n", rulecnt, ipv6addrbuf);
 339                         printf("RULE_BMR=%d\n", rulecnt);
 340                 }
 341
 342                 if (ipv4addr.s_addr) {
 343                         printf("RULE_%d_IPV4ADDR=%s\n", rulecnt, ipv4addrbuf);
 344                         printf("RULE_%d_ADDR4LEN=%d\n", rulecnt, addr4len);
 345                 }
 346
 347
 348                 if (psidlen > 0 && 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
 354                                 if (start == 0)
 355                                         start = 1;
 356
 357                                 if (start <= end)
 358                                         printf("%d-%d ", start, end);
 359                         }
 360                         printf("'\n");
 361                 }
 362
 363                 if (dmr)
 364                         printf("RULE_%d_DMR=%s\n", rulecnt, dmr);
 365
 366                 if (br)
 367                         printf("RULE_%d_BR=%s\n", rulecnt, br);
 368         }
 369
 370         printf("RULE_COUNT=%d\n", rulecnt);
 371         return status;
 372 }