add SO_BINDTODEVICE support
[project/mdnsd.git] / interface.c
1 /*
2  * Copyright (C) 2014 John Crispin <blogic@openwrt.org>
3  * Copyright (C) 2014 Felix Fietkau <nbd@openwrt.org>
4  *
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
8  *
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.
13  */
14
15 #define _GNU_SOURCE
16 #include <sys/socket.h>
17 #include <sys/ioctl.h>
18 #include <sys/types.h>
19 #include <sys/stat.h>
20 #include <sys/utsname.h>
21 #include <net/if.h>
22 #include <netinet/in.h>
23 #include <arpa/inet.h>
24 #include <sys/types.h>
25
26 #include <ifaddrs.h>
27 #include <stdlib.h>
28 #include <string.h>
29 #include <unistd.h>
30 #include <stdio.h>
31 #include <errno.h>
32
33 #include <libubox/usock.h>
34 #include <libubox/uloop.h>
35 #include <libubox/avl-cmp.h>
36 #include <libubox/utils.h>
37 #include "interface.h"
38 #include "util.h"
39 #include "dns.h"
40 #include "announce.h"
41
42 static int
43 interface_send_packet4(struct interface *iface, struct iovec *iov, int iov_len)
44 {
45         static size_t cmsg_data[( CMSG_SPACE(sizeof(struct in_pktinfo)) / sizeof(size_t)) + 1];
46         static struct sockaddr_in a;
47         static struct msghdr m = {
48                 .msg_name = (struct sockaddr *) &a,
49                 .msg_namelen = sizeof(a),
50                 .msg_control = cmsg_data,
51                 .msg_controllen = CMSG_LEN(sizeof(struct in_pktinfo)),
52         };
53         struct in_pktinfo *pkti;
54         struct cmsghdr *cmsg;
55         int fd = iface->fd.fd;
56
57         a.sin_family = AF_INET;
58         a.sin_port = htons(MCAST_PORT);
59         m.msg_iov = iov;
60         m.msg_iovlen = iov_len;
61
62         memset(cmsg_data, 0, sizeof(cmsg_data));
63         cmsg = CMSG_FIRSTHDR(&m);
64         cmsg->cmsg_len = m.msg_controllen;
65         cmsg->cmsg_level = IPPROTO_IP;
66         cmsg->cmsg_type = IP_PKTINFO;
67
68         pkti = (struct in_pktinfo*) CMSG_DATA(cmsg);
69         pkti->ipi_ifindex = iface->ifindex;
70
71         a.sin_addr.s_addr = inet_addr(MCAST_ADDR);
72
73         return sendmsg(fd, &m, 0);
74 }
75
76 static int
77 interface_send_packet6(struct interface *iface, struct iovec *iov, int iov_len)
78 {
79         static size_t cmsg_data[( CMSG_SPACE(sizeof(struct in6_pktinfo)) / sizeof(size_t)) + 1];
80         static struct sockaddr_in6 a;
81         static struct msghdr m = {
82                 .msg_name = (struct sockaddr *) &a,
83                 .msg_namelen = sizeof(a),
84                 .msg_control = cmsg_data,
85                 .msg_controllen = CMSG_LEN(sizeof(struct in6_pktinfo)),
86         };
87         struct in6_pktinfo *pkti;
88         struct cmsghdr *cmsg;
89         int fd = iface->fd.fd;
90
91         a.sin6_family = AF_INET6;
92         a.sin6_port = htons(MCAST_PORT);
93         m.msg_iov = iov;
94         m.msg_iovlen = iov_len;
95
96         memset(cmsg_data, 0, sizeof(cmsg_data));
97         cmsg = CMSG_FIRSTHDR(&m);
98         cmsg->cmsg_len = m.msg_controllen;
99         cmsg->cmsg_level = IPPROTO_IPV6;
100         cmsg->cmsg_type = IPV6_PKTINFO;
101
102         pkti = (struct in6_pktinfo*) CMSG_DATA(cmsg);
103         pkti->ipi6_ifindex = iface->ifindex;
104
105         inet_pton(AF_INET6, MCAST_ADDR6, &a.sin6_addr);
106
107         return sendmsg(fd, &m, 0);
108 }
109
110 int
111 interface_send_packet(struct interface *iface, struct iovec *iov, int iov_len)
112 {
113         if (debug > 1) {
114                 fprintf(stderr, "TX ipv%d: %s\n", iface->v6 * 2 + 4, iface->name);
115                 fprintf(stderr, "  multicast: %d\n", iface->multicast);
116         }
117
118         if (iface->v6)
119                 return interface_send_packet6(iface, iov, iov_len);
120
121         return interface_send_packet4(iface, iov, iov_len);
122 }
123
124 static void interface_close(struct interface *iface)
125 {
126         if (iface->fd.fd < 0)
127                 return;
128
129         announce_free(iface);
130         uloop_fd_delete(&iface->fd);
131         close(iface->fd.fd);
132         iface->fd.fd = -1;
133 }
134
135 static void interface_free(struct interface *iface)
136 {
137         interface_close(iface);
138         free(iface);
139 }
140
141 static void
142 read_socket4(struct uloop_fd *u, unsigned int events)
143 {
144         struct interface *iface = container_of(u, struct interface, fd);
145         static uint8_t buffer[8 * 1024];
146         struct iovec iov[1];
147         char cmsg[CMSG_SPACE(sizeof(struct in_pktinfo)) + CMSG_SPACE(sizeof(int)) + 1];
148         struct cmsghdr *cmsgptr;
149         struct msghdr msg;
150         socklen_t len;
151         struct sockaddr_in from;
152         int flags = 0, ifindex = -1;
153         uint8_t ttl = 0;
154         struct in_pktinfo *inp = NULL;
155
156         if (u->eof) {
157                 interface_close(iface);
158                 uloop_timeout_set(&iface->reconnect, 1000);
159                 return;
160         }
161
162         iov[0].iov_base = buffer;
163         iov[0].iov_len = sizeof(buffer);
164
165         memset(&msg, 0, sizeof(msg));
166         msg.msg_name = (struct sockaddr *) &from;
167         msg.msg_namelen = sizeof(struct sockaddr_in);
168         msg.msg_iov = iov;
169         msg.msg_iovlen = 1;
170         msg.msg_control = &cmsg;
171         msg.msg_controllen = sizeof(cmsg);
172
173         len = recvmsg(u->fd, &msg, flags);
174         if (len == -1) {
175                 perror("read failed");
176                 return;
177         }
178         for (cmsgptr = CMSG_FIRSTHDR(&msg); cmsgptr != NULL; cmsgptr = CMSG_NXTHDR(&msg, cmsgptr)) {
179                 void *c = CMSG_DATA(cmsgptr);
180
181                 switch (cmsgptr->cmsg_type) {
182                 case IP_PKTINFO:
183                         inp = ((struct in_pktinfo *) c);
184                         break;
185
186                 case IP_TTL:
187                         ttl = (uint8_t) *((int *) c);
188                         break;
189
190                 default:
191                         fprintf(stderr, "unknown cmsg %x\n", cmsgptr->cmsg_type);
192                         return;
193                 }
194         }
195
196         if (ttl != 255)
197                 return;
198
199         if (debug > 1) {
200                 char buf[256];
201
202                 fprintf(stderr, "RX ipv4: %s\n", iface->name);
203                 fprintf(stderr, "  multicast: %d\n", iface->multicast);
204                 inet_ntop(AF_INET, &from.sin_addr, buf, 256);
205                 fprintf(stderr, "  src %s:%d\n", buf, from.sin_port);
206                 inet_ntop(AF_INET, &inp->ipi_spec_dst, buf, 256);
207                 fprintf(stderr, "  dst %s\n", buf);
208                 inet_ntop(AF_INET, &inp->ipi_addr, buf, 256);
209                 fprintf(stderr, "  real %s\n", buf);
210         }
211
212         if (inp->ipi_ifindex != iface->ifindex)
213                 fprintf(stderr, "invalid iface index %d != %d\n", ifindex, iface->ifindex);
214         else
215                 dns_handle_packet(iface, buffer, len, 0);
216 }
217
218 static void
219 read_socket6(struct uloop_fd *u, unsigned int events)
220 {
221         struct interface *iface = container_of(u, struct interface, fd);
222         static uint8_t buffer[8 * 1024];
223         struct iovec iov[1];
224         char cmsg6[CMSG_SPACE(sizeof(struct in6_pktinfo)) + CMSG_SPACE(sizeof(int)) + 1];
225         struct cmsghdr *cmsgptr;
226         struct msghdr msg;
227         socklen_t len;
228         struct sockaddr_in6 from;
229         int flags = 0, ifindex = -1;
230         int ttl = 0;
231         struct in6_pktinfo *inp = NULL;
232
233         if (u->eof) {
234                 interface_close(iface);
235                 uloop_timeout_set(&iface->reconnect, 1000);
236                 return;
237         }
238
239         iov[0].iov_base = buffer;
240         iov[0].iov_len = sizeof(buffer);
241
242         memset(&msg, 0, sizeof(msg));
243         msg.msg_name = (struct sockaddr *) &from;
244         msg.msg_namelen = sizeof(struct sockaddr_in6);
245         msg.msg_iov = iov;
246         msg.msg_iovlen = 1;
247         msg.msg_control = &cmsg6;
248         msg.msg_controllen = sizeof(cmsg6);
249
250         len = recvmsg(u->fd, &msg, flags);
251         if (len == -1) {
252                 perror("read failed");
253                 return;
254         }
255         for (cmsgptr = CMSG_FIRSTHDR(&msg); cmsgptr != NULL; cmsgptr = CMSG_NXTHDR(&msg, cmsgptr)) {
256                 void *c = CMSG_DATA(cmsgptr);
257
258                 switch (cmsgptr->cmsg_type) {
259                 case IPV6_PKTINFO:
260                         inp = ((struct in6_pktinfo *) c);
261                         break;
262
263                 case IPV6_HOPLIMIT:
264                         ttl = (uint8_t) *((int *) c);
265                         break;
266
267                 default:
268                         fprintf(stderr, "unknown cmsg %x\n", cmsgptr->cmsg_type);
269                         return;
270                 }
271         }
272
273         if (ttl != 255)
274                 return;
275
276         if (debug > 1) {
277                 char buf[256];
278
279                 fprintf(stderr, "RX ipv6: %s\n", iface->name);
280                 fprintf(stderr, "  multicast: %d\n", iface->multicast);
281                 inet_ntop(AF_INET6, &from.sin6_addr, buf, 256);
282                 fprintf(stderr, "  src %s:%d\n", buf, from.sin6_port);
283                 inet_ntop(AF_INET6, &inp->ipi6_addr, buf, 256);
284                 fprintf(stderr, "  dst %s\n", buf);
285         }
286
287         if (inp->ipi6_ifindex != iface->ifindex)
288                 fprintf(stderr, "invalid iface index %d != %d\n", ifindex, iface->ifindex);
289         else
290                 dns_handle_packet(iface, buffer, len, 0);
291 }
292
293 static int
294 interface_mcast_setup4(struct interface *iface)
295 {
296         struct ip_mreqn mreq;
297         uint8_t ttl = 255;
298         int no = 0;
299         struct sockaddr_in sa = { 0 };
300         int fd = iface->fd.fd;
301
302         sa.sin_family = AF_INET;
303         sa.sin_port = htons(MCAST_PORT);
304         inet_pton(AF_INET, MCAST_ADDR, &sa.sin_addr);
305
306         memset(&mreq, 0, sizeof(mreq));
307         mreq.imr_address.s_addr = iface->v4_addr.s_addr;
308         mreq.imr_multiaddr = sa.sin_addr;
309         mreq.imr_ifindex = iface->ifindex;
310
311         if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl)) < 0)
312                 fprintf(stderr, "ioctl failed: IP_MULTICAST_TTL\n");
313
314         /* Some network drivers have issues with dropping membership of
315          * mcast groups when the iface is down, but don't allow rejoining
316          * when it comes back up. This is an ugly workaround
317          * -- this was copied from avahi --
318          */
319         setsockopt(fd, IPPROTO_IP, IP_DROP_MEMBERSHIP, &mreq, sizeof(mreq));
320
321         if (setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0) {
322                 fprintf(stderr, "failed to join multicast group: %s\n", strerror(errno));
323                 close(fd);
324                 fd = -1;
325                 return -1;
326         }
327
328         if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, &no, sizeof(no)) < 0)
329                 fprintf(stderr, "ioctl failed: IP_MULTICAST_LOOP\n");
330
331         return 0;
332 }
333
334 static int
335 interface_socket_setup6(struct interface *iface)
336 {
337         struct ipv6_mreq mreq;
338         int ttl = 255;
339         int no = 0;
340         struct sockaddr_in6 sa = { 0 };
341         int fd = iface->fd.fd;
342
343         sa.sin6_family = AF_INET6;
344         sa.sin6_port = htons(MCAST_PORT);
345         inet_pton(AF_INET6, MCAST_ADDR6, &sa.sin6_addr);
346
347         memset(&mreq, 0, sizeof(mreq));
348         mreq.ipv6mr_multiaddr = sa.sin6_addr;
349         mreq.ipv6mr_interface = iface->ifindex;
350
351         if (setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_HOPS, &ttl, sizeof(ttl)) < 0)
352                 fprintf(stderr, "ioctl failed: IPV6_MULTICAST_HOPS\n");
353
354         setsockopt(fd, IPPROTO_IPV6, IPV6_LEAVE_GROUP, &mreq, sizeof(mreq));
355         if (setsockopt(fd, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0) {
356                 fprintf(stderr, "failed to join multicast group: %s\n", strerror(errno));
357                 close(fd);
358                 fd = -1;
359                 return -1;
360         }
361
362         if (setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &no, sizeof(no)) < 0)
363                 fprintf(stderr, "ioctl failed: IPV6_MULTICAST_LOOP\n");
364
365         return 0;
366 }
367
368 static void
369 reconnect_socket4(struct uloop_timeout *timeout)
370 {
371         struct interface *iface = container_of(timeout, struct interface, reconnect);
372         int yes = 1;
373
374         iface->fd.fd = usock(USOCK_UDP | USOCK_SERVER | USOCK_NONBLOCK | USOCK_IPV4ONLY,
375                 (iface->multicast) ? (iface->mcast_addr) : (iface->v4_addrs), "5353");
376         if (iface->fd.fd < 0) {
377                 fprintf(stderr, "failed to add listener %s: %s\n", iface->mcast_addr, strerror(errno));
378                 goto retry;
379         }
380
381         if (setsockopt(iface->fd.fd, SOL_SOCKET, SO_BINDTODEVICE, iface->name, strlen(iface->name) < 0))
382                 fprintf(stderr, "ioctl failed: SO_BINDTODEVICE\n");
383
384         if (setsockopt(iface->fd.fd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes)) < 0)
385                 fprintf(stderr, "ioctl failed: SO_REUSEADDR\n");
386
387         if (setsockopt(iface->fd.fd, IPPROTO_IP, IP_RECVTTL, &yes, sizeof(yes)) < 0)
388                 fprintf(stderr, "ioctl failed: IP_RECVTTL\n");
389
390         if (setsockopt(iface->fd.fd, IPPROTO_IP, IP_PKTINFO, &yes, sizeof(yes)) < 0)
391                 fprintf(stderr, "ioctl failed: IP_PKTINFO\n");
392
393         if (iface->multicast && interface_mcast_setup4(iface)) {
394                 iface->fd.fd = -1;
395                 goto retry;
396         }
397
398         uloop_fd_add(&iface->fd, ULOOP_READ);
399         if (iface->multicast) {
400                 dns_send_question(iface, "_services._dns-sd._udp.local", TYPE_PTR, 1);
401                 announce_init(iface);
402         }
403
404         return;
405
406 retry:
407         uloop_timeout_set(timeout, 1000);
408 }
409
410 static void
411 reconnect_socket6(struct uloop_timeout *timeout)
412 {
413         struct interface *iface = container_of(timeout, struct interface, reconnect);
414         char mcast_addr[128];
415         int ttl = 255;
416         int yes = 1;
417
418         snprintf(mcast_addr, sizeof(mcast_addr), "%s%%%s", (iface->multicast) ? (iface->mcast_addr) : (iface->v6_addrs), iface->name);
419         iface->fd.fd = usock(USOCK_UDP | USOCK_SERVER | USOCK_NONBLOCK | USOCK_IPV6ONLY, mcast_addr, "5353");
420         if (iface->fd.fd < 0) {
421                 fprintf(stderr, "failed to add listener %s: %s\n", mcast_addr, strerror(errno));
422                 goto retry;
423         }
424
425         if (setsockopt(iface->fd.fd, SOL_SOCKET, SO_BINDTODEVICE, iface->name, strlen(iface->name) < 0))
426                 fprintf(stderr, "ioctl failed: SO_BINDTODEVICE\n");
427
428         if (setsockopt(iface->fd.fd, IPPROTO_IPV6, IPV6_UNICAST_HOPS, &ttl, sizeof(ttl)) < 0)
429                 fprintf(stderr, "ioctl failed: IPV6_UNICAST_HOPS\n");
430
431         if (setsockopt(iface->fd.fd, IPPROTO_IPV6, IPV6_RECVPKTINFO, &yes, sizeof(yes)) < 0)
432                 fprintf(stderr, "ioctl failed: IPV6_RECVPKTINFO\n");
433
434         if (setsockopt(iface->fd.fd, IPPROTO_IPV6, IPV6_RECVHOPLIMIT, &yes, sizeof(yes)) < 0)
435                 fprintf(stderr, "ioctl failed: IPV6_RECVHOPLIMIT\n");
436
437         if (setsockopt(iface->fd.fd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes)) < 0)
438                 fprintf(stderr, "ioctl failed: SO_REUSEADDR\n");
439
440         if (iface->multicast && interface_socket_setup6(iface)) {
441                 iface->fd.fd = -1;
442                 goto retry;
443         }
444
445         uloop_fd_add(&iface->fd, ULOOP_READ);
446
447         if (iface->multicast) {
448                 dns_send_question(iface, "_services._dns-sd._udp.local", TYPE_PTR, 1);
449                 announce_init(iface);
450         }
451
452         return;
453
454 retry:
455         uloop_timeout_set(timeout, 1000);
456 }
457
458
459 static void interface_start(struct interface *iface)
460 {
461         if (iface->v6) {
462                 iface->fd.cb = read_socket6;
463                 iface->reconnect.cb = reconnect_socket6;
464         } else {
465                 iface->fd.cb = read_socket4;
466                 iface->reconnect.cb = reconnect_socket4;
467         }
468         uloop_timeout_set(&iface->reconnect, 100);
469 }
470
471 static void
472 iface_update_cb(struct vlist_tree *tree, struct vlist_node *node_new,
473                 struct vlist_node *node_old)
474 {
475         struct interface *iface;
476
477         if (node_old) {
478                 iface = container_of(node_old, struct interface, node);
479                 interface_free(iface);
480         }
481
482         if (node_new) {
483                 iface = container_of(node_new, struct interface, node);
484                 interface_start(iface);
485         }
486 }
487
488 static struct interface* _interface_add(const char *name, int multicast, int v6)
489 {
490         struct interface *iface;
491         char *name_buf;
492         char *id_buf;
493
494         iface = calloc_a(sizeof(*iface),
495                 &name_buf, strlen(name) + 1,
496                 &id_buf, strlen(name) + 5);
497
498         sprintf(id_buf, "%d_%d_%s", multicast, v6, name);
499         iface->name = strcpy(name_buf, name);
500         iface->id = id_buf;
501         iface->ifindex = if_nametoindex(name);
502         iface->fd.fd = -1;
503         iface->multicast = multicast;
504         iface->v6 = v6;
505         if (v6)
506                 iface->mcast_addr = MCAST_ADDR6;
507         else
508                 iface->mcast_addr = MCAST_ADDR;
509
510         if (iface->ifindex <= 0)
511                 goto error;
512
513         vlist_add(&interfaces, &iface->node, iface->id);
514         return iface;
515
516 error:
517         free(iface);
518         return NULL;
519 }
520
521 int interface_add(const char *name)
522 {
523         struct interface *v4 = NULL, *v6 = NULL, *unicast;
524         struct ifaddrs *ifap, *ifa;
525
526         getifaddrs(&ifap);
527
528         for (ifa = ifap; ifa; ifa = ifa->ifa_next) {
529                 if (strcmp(ifa->ifa_name, name))
530                         continue;
531                 if (ifa->ifa_addr->sa_family == AF_INET && !v4) {
532                         struct sockaddr_in *sa;
533
534                         if (cfg_proto && (cfg_proto != 4))
535                                 continue;
536
537                         unicast = _interface_add(name, 0, 0);
538                         if (!unicast)
539                                 continue;
540                         sa = (struct sockaddr_in *) ifa->ifa_addr;
541                         memcpy(&unicast->v4_addr, &sa->sin_addr, sizeof(unicast->v4_addr));
542                         inet_ntop(AF_INET, &sa->sin_addr, unicast->v4_addrs, sizeof(unicast->v4_addrs));
543
544                         v4 = _interface_add(name, 1, 0);
545                         if (!v4)
546                                 continue;
547                         memcpy(&v4->v4_addr, &sa->sin_addr, sizeof(v4->v4_addr));
548                         inet_ntop(AF_INET, &sa->sin_addr, v4->v4_addrs, sizeof(v4->v4_addrs));
549
550                         v4->peer = unicast;
551                         unicast->peer = v4;
552                 }
553
554                 if (ifa->ifa_addr->sa_family == AF_INET6 && !v6) {
555                         uint8_t ll_prefix[] = {0xfe, 0x80 };
556                         struct sockaddr_in6 *sa6;
557
558                         if (cfg_proto && (cfg_proto != 6))
559                                 continue;
560
561                         sa6 = (struct sockaddr_in6 *) ifa->ifa_addr;
562                         if (memcmp(&sa6->sin6_addr, &ll_prefix, 2))
563                                 continue;
564
565                         unicast = _interface_add(name, 0, 1);
566                         if (!unicast)
567                                 continue;
568                         memcpy(&unicast->v6_addr, &sa6->sin6_addr, sizeof(unicast->v6_addr));
569                         inet_ntop(AF_INET6, &sa6->sin6_addr, unicast->v6_addrs, sizeof(unicast->v6_addrs));
570
571                         v6 = _interface_add(name, 1, 1);
572                         if (!v6)
573                                 continue;
574                         memcpy(&v6->v6_addr, &sa6->sin6_addr, sizeof(v6->v6_addr));
575                         inet_ntop(AF_INET6, &sa6->sin6_addr, v6->v6_addrs, sizeof(v6->v6_addrs));
576
577                         v6->peer = unicast;
578                         unicast->peer = v6;
579                 }
580         }
581
582         freeifaddrs(ifap);
583
584         return !v4 && !v6;
585 }
586
587 VLIST_TREE(interfaces, avl_strcmp, iface_update_cb, false, false);