2 * mapcalc - MAP parameter calculation
4 * Author: Steven Barth <cyrus@openwrt.org>
5 * Copyright (c) 2014-2015 cisco Systems, Inc.
6 * Copyright (c) 2015 Steven Barth <cyrus@openwrt.org>
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
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.
20 #include <arpa/inet.h>
23 #include <libubox/utils.h>
26 struct blob_attr
*dump
= NULL
;
33 static const struct blobmsg_policy dump_attrs
[DUMP_ATTR_MAX
] = {
34 [DUMP_ATTR_INTERFACE
] = { .name
= "interface", .type
= BLOBMSG_TYPE_ARRAY
},
45 static 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
},
58 static 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
},
63 static int bmemcmp(const void *av
, const void *bv
, size_t bits
)
65 const uint8_t *a
= av
, *b
= bv
;
66 size_t bytes
= bits
/ 8;
69 int res
= memcmp(a
, b
, bytes
);
70 if (res
== 0 && bits
> 0)
71 res
= (a
[bytes
] >> (8 - bits
)) - (b
[bytes
] >> (8 - bits
));
76 static void bmemcpy(void *av
, const void *bv
, size_t bits
)
79 const uint8_t *b
= bv
;
81 size_t bytes
= bits
/ 8;
86 uint8_t mask
= (1 << (8 - bits
)) - 1;
87 a
[bytes
] = (a
[bytes
] & mask
) | ((~mask
) & b
[bytes
]);
91 static void bmemcpys64(void *av
, const void *bv
, size_t frombits
, size_t nbits
)
94 const uint8_t *b
= bv
;
95 size_t frombyte
= frombits
/ 8, tobyte
= (frombits
+ nbits
) / 8;
97 memcpy(&buf
, &b
[frombyte
], tobyte
- frombyte
+ 1);
98 buf
= cpu_to_be64(be64_to_cpu(buf
) << (frombits
% 8));
100 bmemcpy(av
, &buf
, nbits
);
103 static void handle_dump(struct ubus_request
*req
__attribute__((unused
)),
104 int type
__attribute__((unused
)), struct blob_attr
*msg
)
106 struct blob_attr
*tb
[DUMP_ATTR_INTERFACE
];
107 blobmsg_parse(dump_attrs
, DUMP_ATTR_MAX
, tb
, blob_data(msg
), blob_len(msg
));
109 if (!tb
[DUMP_ATTR_INTERFACE
])
112 dump
= blob_memdup(tb
[DUMP_ATTR_INTERFACE
]);
115 static void match_prefix(int *pdlen
, struct in6_addr
*pd
, struct blob_attr
*cur
,
116 const struct in6_addr
*ipv6prefix
, int prefix6len
, bool lw4o6
)
121 if (!cur
|| blobmsg_type(cur
) != BLOBMSG_TYPE_ARRAY
|| !blobmsg_check_attr(cur
, false))
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
));
129 if (!ptb
[PREFIX_ATTR_ADDRESS
] || !ptb
[PREFIX_ATTR_MASK
])
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
);
136 // lw4over6 /128-address-as-PD matching madness workaround
137 if (lw4o6
&& mask
== 128)
140 if (*pdlen
< mask
&& mask
>= prefix6len
&&
141 !bmemcmp(&prefix
, ipv6prefix
, prefix6len
)) {
142 bmemcpy(pd
, &prefix
, mask
);
144 } else if (lw4o6
&& *pdlen
< prefix6len
&& mask
< prefix6len
&&
145 !bmemcmp(&prefix
, ipv6prefix
, mask
)) {
146 bmemcpy(pd
, ipv6prefix
, prefix6len
);
170 static char *const token
[] = {
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",
184 [OPT_PDLEN
] = "pdlen",
189 int main(int argc
, char *argv
[])
192 const char *iface
= argv
[1];
194 const char *legacy_env
= getenv("LEGACY");
195 bool legacy
= legacy_env
&& atoi(legacy_env
);
199 fprintf(stderr
, "Usage: %s <interface|*> <rule1> [rule2] [...]\n", argv
[0]);
203 uint32_t network_interface
;
204 struct ubus_context
*ubus
= ubus_connect(NULL
);
206 ubus_lookup_id(ubus
, "network.interface", &network_interface
);
207 ubus_invoke(ubus
, network_interface
, "dump", NULL
, handle_dump
, NULL
, 5000);
211 for (int i
= 2; i
< argc
; ++i
) {
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
;
228 const char *dmr
= NULL
;
229 const char *br
= NULL
;
231 for (char *rule
= strdup(argv
[i
]); *rule
; ) {
234 int idx
= getsubopt(&rule
, token
, &value
);
237 if (idx
== OPT_TYPE
) {
238 lw4o6
= (value
&& !strcmp(value
, "lw4o6"));
239 } else if (idx
== OPT_FMR
) {
241 } else if (idx
== OPT_EALEN
&& (intval
= strtoul(value
, NULL
, 0)) <= 48 && !errno
) {
243 } else if (idx
== OPT_PREFIX4LEN
&& (intval
= strtoul(value
, NULL
, 0)) <= 32 && !errno
) {
245 } else if (idx
== OPT_PREFIX6LEN
&& (intval
= strtoul(value
, NULL
, 0)) <= 128 && !errno
) {
247 } else if (idx
== OPT_IPV4PREFIX
&& inet_pton(AF_INET
, value
, &ipv4prefix
) == 1) {
249 } else if (idx
== OPT_IPV6PREFIX
&& inet_pton(AF_INET6
, value
, &ipv6prefix
) == 1) {
251 } else if (idx
== OPT_PD
&& inet_pton(AF_INET6
, value
, &pd
) == 1) {
253 } else if (idx
== OPT_OFFSET
&& (intval
= strtoul(value
, NULL
, 0)) <= 16 && !errno
) {
255 } else if (idx
== OPT_PSIDLEN
&& (intval
= strtoul(value
, NULL
, 0)) <= 16 && !errno
) {
257 } else if (idx
== OPT_PDLEN
&& (intval
= strtoul(value
, NULL
, 0)) <= 128 && !errno
) {
259 } else if (idx
== OPT_PSID
&& (intval
= strtoul(value
, NULL
, 0)) <= 65535 && !errno
) {
261 } else if (idx
== OPT_DMR
) {
263 } else if (idx
== OPT_BR
) {
266 if (idx
== -1 || idx
>= OPT_MAX
)
267 fprintf(stderr
, "Skipped invalid option: %s\n", value
);
269 fprintf(stderr
, "Skipped invalid value %s for option %s\n",
275 offset
= (lw4o6
) ? 0 : (legacy
) ? 4 : 6;
277 // LW4over6 doesn't have an EALEN and has no psid-autodetect
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
));
293 if (!tb
[IFACE_ATTR_INTERFACE
] || (strcmp(argv
[1], "*") && strcmp(argv
[1],
294 blobmsg_get_string(tb
[IFACE_ATTR_INTERFACE
]))))
297 match_prefix(&pdlen
, &pd
, tb
[IFACE_ATTR_PREFIX
], &ipv6prefix
, prefix6len
, lw4o6
);
300 match_prefix(&pdlen
, &pd
, tb
[IFACE_ATTR_ADDRESS
], &ipv6prefix
, prefix6len
, lw4o6
);
303 iface
= blobmsg_get_string(tb
[IFACE_ATTR_INTERFACE
]);
309 if (ealen
< 0 && pdlen
>= 0)
310 ealen
= pdlen
- prefix6len
;
313 psidlen
= ealen
- (32 - prefix4len
);
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
);
322 psid
= psid
>> (16 - psidlen
);
323 psid16
= cpu_to_be16(psid
);
324 psid
= psid
<< (16 - psidlen
);
326 if (prefix4len
< 0 || prefix6len
< 0 || ealen
< 0 || ealen
< psidlen
) {
327 fprintf(stderr
, "Skipping invalid or incomplete rule: %s\n", argv
[i
]);
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
);
337 if (prefix4len
+ ealen
< 32)
338 addr4len
= prefix4len
+ ealen
;
341 if (pdlen
< 0 && !fmr
) {
342 fprintf(stderr
, "Skipping non-FMR without matching PD: %s\n", argv
[i
]);
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
);
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
];
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
));
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
);
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
);
382 if (ipv4addr
.s_addr
) {
383 printf("RULE_%d_IPV4ADDR=%s\n", rulecnt
, ipv4addrbuf
);
384 printf("RULE_%d_ADDR4LEN=%d\n", rulecnt
, addr4len
);
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;
398 printf("%d-%d ", start
, end
);
404 printf("RULE_%d_DMR=%s\n", rulecnt
, dmr
);
407 printf("RULE_%d_BR=%s\n", rulecnt
, br
);
410 printf("RULE_COUNT=%d\n", rulecnt
);