2 * Copyright (C) 2014 John Crispin <blogic@openwrt.org>
3 * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU Lesser General Public License version 2.1
7 * as published by the Free Software Foundation
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
16 #include <sys/socket.h>
17 #include <sys/ioctl.h>
18 #include <sys/types.h>
20 #include <sys/utsname.h>
22 #include <netinet/in.h>
23 #include <arpa/inet.h>
24 #include <sys/types.h>
33 #include <libubox/usock.h>
34 #include <libubox/uloop.h>
35 #include <libubox/avl-cmp.h>
36 #include <libubox/utils.h>
38 #include "interface.h"
44 static struct uloop_fd ufd
[] = {
45 [SOCK_UC_IPV4
] = { .fd
= -1 },
46 [SOCK_UC_IPV6
] = { .fd
= -1 },
47 [SOCK_MC_IPV4
] = { .fd
= -1 },
48 [SOCK_MC_IPV6
] = { .fd
= -1 },
52 interface_send_packet4(struct interface
*iface
, struct sockaddr_in
*to
, struct iovec
*iov
, int iov_len
)
54 static size_t cmsg_data
[( CMSG_SPACE(sizeof(struct in_pktinfo
)) / sizeof(size_t)) + 1];
55 static struct sockaddr_in a
= {};
56 static struct msghdr m
= {
57 .msg_name
= (struct sockaddr
*) &a
,
58 .msg_namelen
= sizeof(a
),
59 .msg_control
= cmsg_data
,
60 .msg_controllen
= CMSG_LEN(sizeof(struct in_pktinfo
)),
62 struct in_pktinfo
*pkti
;
66 a
.sin_family
= AF_INET
;
67 a
.sin_port
= htons(MCAST_PORT
);
69 m
.msg_iovlen
= iov_len
;
71 memset(cmsg_data
, 0, sizeof(cmsg_data
));
72 cmsg
= CMSG_FIRSTHDR(&m
);
73 cmsg
->cmsg_len
= m
.msg_controllen
;
74 cmsg
->cmsg_level
= IPPROTO_IP
;
75 cmsg
->cmsg_type
= IP_PKTINFO
;
77 pkti
= (struct in_pktinfo
*) CMSG_DATA(cmsg
);
78 pkti
->ipi_ifindex
= iface
->ifindex
;
80 fd
= ufd
[iface
->type
].fd
;
81 if (interface_multicast(iface
)) {
82 a
.sin_addr
.s_addr
= inet_addr(MCAST_ADDR
);
84 fprintf(stderr
, "Ignoring IPv4 address for multicast interface\n");
86 a
.sin_addr
.s_addr
= to
->sin_addr
.s_addr
;
89 return sendmsg(fd
, &m
, 0);
93 interface_send_packet6(struct interface
*iface
, struct sockaddr_in6
*to
, struct iovec
*iov
, int iov_len
)
95 static size_t cmsg_data
[( CMSG_SPACE(sizeof(struct in6_pktinfo
)) / sizeof(size_t)) + 1];
96 static struct sockaddr_in6 a
= {};
97 static struct msghdr m
= {
98 .msg_name
= (struct sockaddr
*) &a
,
99 .msg_namelen
= sizeof(a
),
100 .msg_control
= cmsg_data
,
101 .msg_controllen
= CMSG_LEN(sizeof(struct in6_pktinfo
)),
103 struct in6_pktinfo
*pkti
;
104 struct cmsghdr
*cmsg
;
107 a
.sin6_family
= AF_INET6
;
108 a
.sin6_port
= htons(MCAST_PORT
);
109 a
.sin6_scope_id
= iface
->ifindex
;
111 m
.msg_iovlen
= iov_len
;
113 memset(cmsg_data
, 0, sizeof(cmsg_data
));
114 cmsg
= CMSG_FIRSTHDR(&m
);
115 cmsg
->cmsg_len
= m
.msg_controllen
;
116 cmsg
->cmsg_level
= IPPROTO_IPV6
;
117 cmsg
->cmsg_type
= IPV6_PKTINFO
;
119 pkti
= (struct in6_pktinfo
*) CMSG_DATA(cmsg
);
120 pkti
->ipi6_ifindex
= iface
->ifindex
;
122 fd
= ufd
[iface
->type
].fd
;
123 if (interface_multicast(iface
)) {
124 inet_pton(AF_INET6
, MCAST_ADDR6
, &a
.sin6_addr
);
126 fprintf(stderr
, "Ignoring IPv6 address for multicast interface\n");
128 a
.sin6_addr
= to
->sin6_addr
;
131 return sendmsg(fd
, &m
, 0);
135 interface_send_packet(struct interface
*iface
, struct sockaddr
*to
, struct iovec
*iov
, int iov_len
)
137 if (!interface_multicast(iface
) && !to
) {
138 fprintf(stderr
, "No IP address specified for unicast interface\n");
144 fprintf(stderr
, "TX ipv%d: %s\n", interface_ipv6(iface
) ? 6 : 4, iface
->name
);
145 fprintf(stderr
, " multicast: %d\n", interface_multicast(iface
));
148 if (interface_ipv6(iface
))
149 return interface_send_packet6(iface
, (struct sockaddr_in6
*)to
, iov
, iov_len
);
151 return interface_send_packet4(iface
, (struct sockaddr_in
*)to
, iov
, iov_len
);
154 static struct interface
*interface_lookup(unsigned int ifindex
, enum umdns_socket_type type
)
156 struct interface
*iface
;
158 vlist_for_each_element(&interfaces
, iface
, node
)
159 if (iface
->ifindex
== ifindex
&& iface
->type
== type
)
165 static void interface_free(struct interface
*iface
)
167 cache_cleanup(iface
);
168 announce_free(iface
);
169 free(iface
->addrs
.v4
);
174 interface_valid_src(void *ip1
, void *mask
, void *ip2
, int len
)
184 for (i
= 0; i
< len
; i
++, i1
++, i2
++, m
++) {
185 if ((*i1
& *m
) != (*i2
& *m
))
193 read_socket4(struct uloop_fd
*u
, unsigned int events
)
195 enum umdns_socket_type type
= (enum umdns_socket_type
)(u
- ufd
);
196 struct interface
*iface
;
197 static uint8_t buffer
[8 * 1024];
199 char cmsg
[CMSG_SPACE(sizeof(struct in_pktinfo
)) + CMSG_SPACE(sizeof(int)) + 1];
200 struct cmsghdr
*cmsgptr
;
203 struct sockaddr_in from
;
206 struct in_pktinfo
*inp
= NULL
;
207 bool valid_src
= false;
214 iov
[0].iov_base
= buffer
;
215 iov
[0].iov_len
= sizeof(buffer
);
217 memset(&msg
, 0, sizeof(msg
));
218 msg
.msg_name
= (struct sockaddr
*) &from
;
219 msg
.msg_namelen
= sizeof(struct sockaddr_in
);
222 msg
.msg_control
= &cmsg
;
223 msg
.msg_controllen
= sizeof(cmsg
);
225 len
= recvmsg(u
->fd
, &msg
, flags
);
227 perror("read failed");
230 for (cmsgptr
= CMSG_FIRSTHDR(&msg
); cmsgptr
!= NULL
; cmsgptr
= CMSG_NXTHDR(&msg
, cmsgptr
)) {
231 void *c
= CMSG_DATA(cmsgptr
);
233 switch (cmsgptr
->cmsg_type
) {
235 inp
= ((struct in_pktinfo
*) c
);
239 ttl
= (uint8_t) *((int *) c
);
243 fprintf(stderr
, "unknown cmsg %x\n", cmsgptr
->cmsg_type
);
251 iface
= interface_lookup(inp
->ipi_ifindex
, type
);
258 fprintf(stderr
, "RX ipv4: %s\n", iface
->name
);
259 fprintf(stderr
, " multicast: %d\n", interface_multicast(iface
));
260 inet_ntop(AF_INET
, &from
.sin_addr
, buf
, 256);
261 fprintf(stderr
, " src %s:%d\n", buf
, ntohs(from
.sin_port
));
262 inet_ntop(AF_INET
, &inp
->ipi_spec_dst
, buf
, 256);
263 fprintf(stderr
, " dst %s\n", buf
);
264 inet_ntop(AF_INET
, &inp
->ipi_addr
, buf
, 256);
265 fprintf(stderr
, " real %s\n", buf
);
266 fprintf(stderr
, " ttl %u\n", ttl
);
269 for (size_t i
= 0; i
< iface
->addrs
.n_addr
; i
++) {
270 if (!interface_valid_src((void *)&iface
->addrs
.v4
[i
].addr
,
271 (void *)&iface
->addrs
.v4
[i
].mask
,
272 (void *) &from
.sin_addr
, 4)) {
281 dns_handle_packet(iface
, (struct sockaddr
*) &from
, ntohs(from
.sin_port
), buffer
, len
);
285 read_socket6(struct uloop_fd
*u
, unsigned int events
)
287 enum umdns_socket_type type
= (enum umdns_socket_type
)(u
- ufd
);
288 struct interface
*iface
;
289 static uint8_t buffer
[8 * 1024];
291 char cmsg6
[CMSG_SPACE(sizeof(struct in6_pktinfo
)) + CMSG_SPACE(sizeof(int)) + 1];
292 struct cmsghdr
*cmsgptr
;
295 struct sockaddr_in6 from
;
298 struct in6_pktinfo
*inp
= NULL
;
299 bool valid_src
= false;
306 iov
[0].iov_base
= buffer
;
307 iov
[0].iov_len
= sizeof(buffer
);
309 memset(&msg
, 0, sizeof(msg
));
310 msg
.msg_name
= (struct sockaddr
*) &from
;
311 msg
.msg_namelen
= sizeof(struct sockaddr_in6
);
314 msg
.msg_control
= &cmsg6
;
315 msg
.msg_controllen
= sizeof(cmsg6
);
317 len
= recvmsg(u
->fd
, &msg
, flags
);
319 perror("read failed");
322 for (cmsgptr
= CMSG_FIRSTHDR(&msg
); cmsgptr
!= NULL
; cmsgptr
= CMSG_NXTHDR(&msg
, cmsgptr
)) {
323 void *c
= CMSG_DATA(cmsgptr
);
325 switch (cmsgptr
->cmsg_type
) {
327 inp
= ((struct in6_pktinfo
*) c
);
331 ttl
= (uint8_t) *((int *) c
);
335 fprintf(stderr
, "unknown cmsg %x\n", cmsgptr
->cmsg_type
);
343 iface
= interface_lookup(inp
->ipi6_ifindex
, type
);
350 fprintf(stderr
, "RX ipv6: %s\n", iface
->name
);
351 fprintf(stderr
, " multicast: %d\n", interface_multicast(iface
));
352 inet_ntop(AF_INET6
, &from
.sin6_addr
, buf
, 256);
353 fprintf(stderr
, " src %s:%d\n", buf
, ntohs(from
.sin6_port
));
354 inet_ntop(AF_INET6
, &inp
->ipi6_addr
, buf
, 256);
355 fprintf(stderr
, " dst %s\n", buf
);
356 fprintf(stderr
, " ttl %u\n", ttl
);
359 for (size_t i
= 0; i
< iface
->addrs
.n_addr
; i
++) {
360 if (!interface_valid_src((void *)&iface
->addrs
.v6
[i
].addr
,
361 (void *)&iface
->addrs
.v6
[i
].mask
,
362 (void *)&from
.sin6_addr
, 6)) {
371 dns_handle_packet(iface
, (struct sockaddr
*) &from
, ntohs(from
.sin6_port
), buffer
, len
);
375 interface_mcast_setup4(struct interface
*iface
)
377 struct ip_mreqn mreq
;
378 struct sockaddr_in sa
= {};
379 int fd
= ufd
[SOCK_MC_IPV4
].fd
;
381 sa
.sin_family
= AF_INET
;
382 sa
.sin_port
= htons(MCAST_PORT
);
383 inet_pton(AF_INET
, MCAST_ADDR
, &sa
.sin_addr
);
385 memset(&mreq
, 0, sizeof(mreq
));
386 mreq
.imr_multiaddr
= sa
.sin_addr
;
387 mreq
.imr_ifindex
= iface
->ifindex
;
388 mreq
.imr_address
.s_addr
= iface
->addrs
.v4
[0].addr
.s_addr
;
390 /* Some network drivers have issues with dropping membership of
391 * mcast groups when the iface is down, but don't allow rejoining
392 * when it comes back up. This is an ugly workaround
393 * -- this was copied from avahi --
395 setsockopt(fd
, IPPROTO_IP
, IP_DROP_MEMBERSHIP
, &mreq
, sizeof(mreq
));
396 setsockopt(fd
, IPPROTO_IP
, IP_ADD_MEMBERSHIP
, &mreq
, sizeof(mreq
));
402 interface_mcast_setup6(struct interface
*iface
)
404 struct ipv6_mreq mreq
;
405 struct sockaddr_in6 sa
= {};
406 int fd
= ufd
[SOCK_MC_IPV6
].fd
;
408 sa
.sin6_family
= AF_INET6
;
409 sa
.sin6_port
= htons(MCAST_PORT
);
410 inet_pton(AF_INET6
, MCAST_ADDR6
, &sa
.sin6_addr
);
412 memset(&mreq
, 0, sizeof(mreq
));
413 mreq
.ipv6mr_multiaddr
= sa
.sin6_addr
;
414 mreq
.ipv6mr_interface
= iface
->ifindex
;
416 setsockopt(fd
, IPPROTO_IPV6
, IPV6_LEAVE_GROUP
, &mreq
, sizeof(mreq
));
417 setsockopt(fd
, IPPROTO_IPV6
, IPV6_ADD_MEMBERSHIP
, &mreq
, sizeof(mreq
));
422 static void interface_start(struct interface
*iface
)
424 if (iface
->type
& SOCKTYPE_BIT_UNICAST
)
427 if (iface
->type
& SOCKTYPE_BIT_IPV6
)
428 interface_mcast_setup6(iface
);
430 interface_mcast_setup4(iface
);
432 dns_send_question(iface
, NULL
, C_DNS_SD
, TYPE_PTR
, 0);
433 announce_init(iface
);
437 iface_equal(struct interface
*if_old
, struct interface
*if_new
)
441 if (if_old
->ifindex
!= if_new
->ifindex
||
442 if_old
->addrs
.n_addr
!= if_new
->addrs
.n_addr
)
445 if (if_old
->type
& SOCKTYPE_BIT_IPV6
)
446 addr_size
= sizeof(*if_old
->addrs
.v6
);
448 addr_size
= sizeof(*if_old
->addrs
.v4
);
449 addr_size
*= if_old
->addrs
.n_addr
;
450 if (memcmp(if_old
->addrs
.v4
, if_new
->addrs
.v4
, addr_size
) != 0)
457 iface_update_cb(struct vlist_tree
*tree
, struct vlist_node
*node_new
,
458 struct vlist_node
*node_old
)
460 struct interface
*if_old
= container_of_safe(node_old
, struct interface
, node
);
461 struct interface
*if_new
= container_of_safe(node_new
, struct interface
, node
);
463 if (if_old
&& if_new
) {
464 if (!iface_equal(if_old
, if_new
))
465 cache_cleanup(if_old
);
466 free(if_old
->addrs
.v4
);
467 if_old
->addrs
= if_new
->addrs
;
468 if_old
->ifindex
= if_new
->ifindex
;
474 interface_free(if_old
);
477 interface_start(if_new
);
480 static int interface_init_socket(enum umdns_socket_type type
)
482 struct sockaddr_in6 local6
= {
483 .sin6_family
= AF_INET6
485 struct sockaddr_in local
= {
486 .sin_family
= AF_INET
493 int af
= (type
& SOCKTYPE_BIT_IPV6
) ? AF_INET6
: AF_INET
;
495 if (ufd
[type
].fd
>= 0)
498 ufd
[type
].fd
= fd
= socket(af
, SOCK_DGRAM
, 0);
502 setsockopt(fd
, SOL_SOCKET
, SO_REUSEADDR
, &yes
, sizeof(yes
));
504 setsockopt(fd
, SOL_SOCKET
, SO_REUSEPORT
, &yes
, sizeof(yes
));
512 setsockopt(fd
, IPPROTO_IP
, IP_MULTICAST_TTL
, &ttl
, sizeof(ttl
));
513 setsockopt(fd
, IPPROTO_IP
, IP_TTL
, &ittl
, sizeof(ittl
));
514 setsockopt(fd
, IPPROTO_IP
, IP_MULTICAST_LOOP
, &no
, sizeof(no
));
515 local
.sin_port
= htons(MCAST_PORT
);
518 setsockopt(fd
, IPPROTO_IPV6
, IPV6_MULTICAST_HOPS
, &ttl
, sizeof(ttl
));
519 setsockopt(fd
, IPPROTO_IPV6
, IPV6_UNICAST_HOPS
, &ttl
, sizeof(ttl
));
520 setsockopt(fd
, IPPROTO_IPV6
, IPV6_V6ONLY
, &yes
, sizeof(yes
));
521 setsockopt(fd
, IPPROTO_IPV6
, IPV6_MULTICAST_LOOP
, &no
, sizeof(no
));
522 local6
.sin6_port
= htons(MCAST_PORT
);
526 if (type
& SOCKTYPE_BIT_IPV6
) {
527 ufd
[type
].cb
= read_socket6
;
528 if (bind(fd
, (struct sockaddr
*)&local6
, sizeof(local6
)) < 0)
531 setsockopt(fd
, IPPROTO_IPV6
, IPV6_RECVPKTINFO
, &yes
, sizeof(yes
));
532 setsockopt(fd
, IPPROTO_IPV6
, IPV6_RECVHOPLIMIT
, &yes
, sizeof(yes
));
534 ufd
[type
].cb
= read_socket4
;
535 if (bind(fd
, (struct sockaddr
*)&local
, sizeof(local
)) < 0)
538 setsockopt(fd
, IPPROTO_IP
, IP_PKTINFO
, &yes
, sizeof(yes
));
539 setsockopt(fd
, IPPROTO_IP
, IP_RECVTTL
, &yes
, sizeof(yes
));
542 uloop_fd_add(&ufd
[type
], ULOOP_READ
);
552 __interface_add(const char *name
, enum umdns_socket_type type
,
553 struct interface_addr_list
*list
)
555 struct interface
*iface
;
556 unsigned int ifindex
;
559 if (interface_init_socket(type
))
562 ifindex
= if_nametoindex(name
);
566 iface
= calloc_a(sizeof(*iface
),
567 &id_buf
, strlen(name
) + 3);
569 sprintf(id_buf
, "%d_%s", type
, name
);
570 iface
->name
= id_buf
+ 2;
571 iface
->ifindex
= ifindex
;
573 iface
->addrs
= *list
;
575 vlist_add(&interfaces
, &iface
->node
, id_buf
);
582 int interface_add(const char *name
)
584 struct ifaddrs
*ifap
, *ifa
;
585 struct interface_addr_list addr4
= {}, addr6
= {};
589 for (ifa
= ifap
; ifa
; ifa
= ifa
->ifa_next
) {
590 if (strcmp(ifa
->ifa_name
, name
))
592 if (ifa
->ifa_addr
->sa_family
== AF_INET
) {
593 struct sockaddr_in
*sin
;
595 if (cfg_proto
&& (cfg_proto
!= 4))
598 addr4
.v4
= realloc(addr4
.v4
, (addr4
.n_addr
+ 1) * sizeof(*addr4
.v4
));
599 sin
= (struct sockaddr_in
*) ifa
->ifa_addr
;
600 addr4
.v4
[addr4
.n_addr
].addr
= sin
->sin_addr
;
601 sin
= (struct sockaddr_in
*) ifa
->ifa_netmask
;
602 addr4
.v4
[addr4
.n_addr
++].mask
= sin
->sin_addr
;
605 if (ifa
->ifa_addr
->sa_family
== AF_INET6
) {
606 uint8_t ll_prefix
[] = {0xfe, 0x80 };
607 struct sockaddr_in6
*sin6
;
609 if (cfg_proto
&& (cfg_proto
!= 6))
612 sin6
= (struct sockaddr_in6
*) ifa
->ifa_addr
;
613 if (memcmp(&sin6
->sin6_addr
, &ll_prefix
, 2))
616 addr6
.v6
= realloc(addr6
.v6
, (addr6
.n_addr
+ 1) * sizeof(*addr6
.v6
));
617 sin6
= (struct sockaddr_in6
*) ifa
->ifa_addr
;
618 addr6
.v6
[addr6
.n_addr
].addr
= sin6
->sin6_addr
;
619 sin6
= (struct sockaddr_in6
*) ifa
->ifa_netmask
;
620 addr6
.v6
[addr6
.n_addr
++].mask
= sin6
->sin6_addr
;
627 size_t addr_size
= addr4
.n_addr
* sizeof(*addr4
.v4
);
628 void *addr_dup
= malloc(addr_size
);
630 memcpy(addr_dup
, addr4
.v4
, addr_size
);
631 __interface_add(name
, SOCK_UC_IPV4
, &addr4
);
633 __interface_add(name
, SOCK_MC_IPV4
, &addr4
);
637 size_t addr_size
= addr6
.n_addr
* sizeof(*addr6
.v6
);
638 void *addr_dup
= malloc(addr_size
);
640 memcpy(addr_dup
, addr6
.v6
, addr_size
);
641 __interface_add(name
, SOCK_UC_IPV6
, &addr6
);
643 __interface_add(name
, SOCK_MC_IPV6
, &addr6
);
646 return !addr4
.n_addr
&& !addr6
.n_addr
;
649 void interface_shutdown(void)
651 struct interface
*iface
;
653 vlist_for_each_element(&interfaces
, iface
, node
)
654 if (interface_multicast(iface
)) {
655 dns_reply_a(iface
, NULL
, 0, NULL
);
656 dns_reply_a_additional(iface
, NULL
, 0);
657 service_announce_services(iface
, NULL
, 0);
660 for (size_t i
= 0; i
< ARRAY_SIZE(ufd
); i
++) {
661 uloop_fd_delete(&ufd
[i
]);
667 struct interface
*interface_get(const char *name
, enum umdns_socket_type type
)
670 snprintf(id_buf
, sizeof(id_buf
), "%d_%s", type
, name
);
671 struct interface
*iface
= vlist_find(&interfaces
, id_buf
, iface
, node
);
675 VLIST_TREE(interfaces
, avl_strcmp
, iface_update_cb
, true, false);