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>
7 * Copyright (c) 2018 Hans Dedecker <dedeckeh@gmail.com>
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
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.
21 #include <arpa/inet.h>
24 #include <libubox/utils.h>
27 struct blob_attr
*dump
= NULL
;
34 static const struct blobmsg_policy dump_attrs
[DUMP_ATTR_MAX
] = {
35 [DUMP_ATTR_INTERFACE
] = { .name
= "interface", .type
= BLOBMSG_TYPE_ARRAY
},
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
},
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
},
64 static int bmemcmp(const void *av
, const void *bv
, size_t bits
)
66 const uint8_t *a
= av
, *b
= bv
;
67 size_t bytes
= bits
/ 8;
70 int res
= memcmp(a
, b
, bytes
);
71 if (res
== 0 && bits
> 0)
72 res
= (a
[bytes
] >> (8 - bits
)) - (b
[bytes
] >> (8 - bits
));
77 static void bmemcpy(void *av
, const void *bv
, size_t bits
)
80 const uint8_t *b
= bv
;
82 size_t bytes
= bits
/ 8;
87 uint8_t mask
= (1 << (8 - bits
)) - 1;
88 a
[bytes
] = (a
[bytes
] & mask
) | ((~mask
) & b
[bytes
]);
92 static void bmemcpys64(void *av
, const void *bv
, size_t frombits
, size_t nbits
)
95 const uint8_t *b
= bv
;
96 size_t frombyte
= frombits
/ 8, tobyte
= (frombits
+ nbits
) / 8;
98 memcpy(&buf
, &b
[frombyte
], tobyte
- frombyte
+ 1);
99 buf
= cpu_to_be64(be64_to_cpu(buf
) << (frombits
% 8));
101 bmemcpy(av
, &buf
, nbits
);
104 static void handle_dump(struct ubus_request
*req
__attribute__((unused
)),
105 int type
__attribute__((unused
)), struct blob_attr
*msg
)
107 struct blob_attr
*tb
[DUMP_ATTR_INTERFACE
];
108 blobmsg_parse(dump_attrs
, DUMP_ATTR_MAX
, tb
, blob_data(msg
), blob_len(msg
));
110 if (!tb
[DUMP_ATTR_INTERFACE
])
113 dump
= blob_memdup(tb
[DUMP_ATTR_INTERFACE
]);
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
)
122 if (!cur
|| blobmsg_type(cur
) != BLOBMSG_TYPE_ARRAY
|| !blobmsg_check_attr(cur
, false))
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
));
130 if (!ptb
[PREFIX_ATTR_ADDRESS
] || !ptb
[PREFIX_ATTR_MASK
])
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
);
137 // lw4over6 /128-address-as-PD matching madness workaround
138 if (lw4o6
&& mask
== 128)
141 if (*pdlen
< mask
&& mask
>= prefix6len
&&
142 !bmemcmp(&prefix
, ipv6prefix
, prefix6len
)) {
143 bmemcpy(pd
, &prefix
, mask
);
145 } else if (lw4o6
&& *pdlen
< prefix6len
&& mask
< prefix6len
&&
146 !bmemcmp(&prefix
, ipv6prefix
, mask
)) {
147 bmemcpy(pd
, ipv6prefix
, prefix6len
);
171 static char *const token
[] = {
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",
185 [OPT_PDLEN
] = "pdlen",
190 int main(int argc
, char *argv
[])
193 const char *iface
= argv
[1];
195 const char *legacy_env
= getenv("LEGACY");
196 bool legacy
= legacy_env
&& atoi(legacy_env
);
200 fprintf(stderr
, "Usage: %s <interface|*> <rule1> [rule2] [...]\n", argv
[0]);
204 uint32_t network_interface
;
205 struct ubus_context
*ubus
= ubus_connect(NULL
);
207 ubus_lookup_id(ubus
, "network.interface", &network_interface
);
208 ubus_invoke(ubus
, network_interface
, "dump", NULL
, handle_dump
, NULL
, 5000);
212 for (int i
= 2; i
< argc
; ++i
) {
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
;
229 const char *dmr
= NULL
;
230 const char *br
= NULL
;
232 for (char *rule
= strdup(argv
[i
]); *rule
; ) {
235 int idx
= getsubopt(&rule
, token
, &value
);
238 if (idx
== OPT_TYPE
) {
239 lw4o6
= (value
&& !strcmp(value
, "lw4o6"));
240 } else if (idx
== OPT_FMR
) {
242 } else if (idx
== OPT_EALEN
&& (intval
= strtoul(value
, NULL
, 0)) <= 48 && !errno
) {
244 } else if (idx
== OPT_PREFIX4LEN
&& (intval
= strtoul(value
, NULL
, 0)) <= 32 && !errno
) {
246 } else if (idx
== OPT_PREFIX6LEN
&& (intval
= strtoul(value
, NULL
, 0)) <= 128 && !errno
) {
248 } else if (idx
== OPT_IPV4PREFIX
&& inet_pton(AF_INET
, value
, &ipv4prefix
) == 1) {
250 } else if (idx
== OPT_IPV6PREFIX
&& inet_pton(AF_INET6
, value
, &ipv6prefix
) == 1) {
252 } else if (idx
== OPT_PD
&& inet_pton(AF_INET6
, value
, &pd
) == 1) {
254 } else if (idx
== OPT_OFFSET
&& (intval
= strtoul(value
, NULL
, 0)) <= 16 && !errno
) {
256 } else if (idx
== OPT_PSIDLEN
&& (intval
= strtoul(value
, NULL
, 0)) <= 16 && !errno
) {
258 } else if (idx
== OPT_PDLEN
&& (intval
= strtoul(value
, NULL
, 0)) <= 128 && !errno
) {
260 } else if (idx
== OPT_PSID
&& (intval
= strtoul(value
, NULL
, 0)) <= 65535 && !errno
) {
262 } else if (idx
== OPT_DMR
) {
264 } else if (idx
== OPT_BR
) {
267 if (idx
== -1 || idx
>= OPT_MAX
)
268 fprintf(stderr
, "Skipped invalid option: %s\n", value
);
270 fprintf(stderr
, "Skipped invalid value %s for option %s\n",
276 offset
= (lw4o6
) ? 0 : (legacy
) ? 4 : 6;
278 // LW4over6 doesn't have an EALEN and has no psid-autodetect
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
));
294 if (!tb
[IFACE_ATTR_INTERFACE
] || (strcmp(argv
[1], "*") && strcmp(argv
[1],
295 blobmsg_get_string(tb
[IFACE_ATTR_INTERFACE
]))))
298 match_prefix(&pdlen
, &pd
, tb
[IFACE_ATTR_PREFIX
], &ipv6prefix
, prefix6len
, lw4o6
);
301 match_prefix(&pdlen
, &pd
, tb
[IFACE_ATTR_ADDRESS
], &ipv6prefix
, prefix6len
, lw4o6
);
304 iface
= blobmsg_get_string(tb
[IFACE_ATTR_INTERFACE
]);
310 if (ealen
< 0 && pdlen
>= 0)
311 ealen
= pdlen
- prefix6len
;
314 psidlen
= ealen
- (32 - prefix4len
);
321 if (prefix4len
< 0 || prefix6len
< 0 || ealen
< 0 || psidlen
> 16 || ealen
< psidlen
) {
322 fprintf(stderr
, "Skipping invalid or incomplete rule: %s\n", argv
[i
]);
327 if (psid
< 0 && psidlen
>= 0 && pdlen
>= 0) {
328 bmemcpys64(&psid16
, &pd
, prefix6len
+ ealen
- psidlen
, psidlen
);
329 psid
= be16_to_cpu(psid16
);
333 psid
= psid
>> (16 - psidlen
);
334 psid16
= cpu_to_be16(psid
);
335 psid
= psid
<< (16 - psidlen
);
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
);
343 if (prefix4len
+ ealen
< 32)
344 addr4len
= prefix4len
+ ealen
;
347 if (pdlen
< 0 && !fmr
) {
348 fprintf(stderr
, "Skipping non-FMR without matching PD: %s\n", argv
[i
]);
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
);
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
];
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
));
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
);
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
);
388 if (ipv4addr
.s_addr
) {
389 printf("RULE_%d_IPV4ADDR=%s\n", rulecnt
, ipv4addrbuf
);
390 printf("RULE_%d_ADDR4LEN=%d\n", rulecnt
, addr4len
);
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;
404 printf("%d-%d ", start
, end
);
410 printf("RULE_%d_DMR=%s\n", rulecnt
, dmr
);
413 printf("RULE_%d_BR=%s\n", rulecnt
, br
);
416 printf("RULE_COUNT=%d\n", rulecnt
);