src/mrib.c

   1 /*
   2  * Author: Steven Barth <steven at midlink.org>
   3  *
   4  * Copyright 2015 Deutsche Telekom AG
   5  *
   6  * Licensed under the Apache License, Version 2.0 (the "License");
   7  * you may not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  *  http://www.apache.org/licenses/LICENSE-2.0
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an "AS IS" BASIS,
  14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  *
  18  */
  19
  20 #include <errno.h>
  21 #include <stdio.h>
  22 #include <string.h>
  23 #include <stdlib.h>
  24 #include <endian.h>
  25 #include <sys/socket.h>
  26 #include <sys/ioctl.h>
  27 #include <unistd.h>
  28 #include <net/if.h>
  29
  30 #include <arpa/inet.h>
  31 #include <netinet/in.h>
  32 #include <netinet/ip.h>
  33 #include <netinet/ip6.h>
  34
  35 #include <linux/mroute.h>
  36 #include <linux/mroute6.h>
  37
  38 #include <libubox/uloop.h>
  39
  40 #include "omcproxy.h"
  41 #include "mrib.h"
  42
  43 struct mrib_route {
  44         struct list_head head;
  45         struct in6_addr group;
  46         struct in6_addr source;
  47         omgp_time_t valid_until;
  48 };
  49
  50 struct mrib_iface {
  51         int ifindex;
  52         struct list_head users;
  53         struct list_head routes;
  54         struct list_head queriers;
  55         struct uloop_timeout timer;
  56 };
  57
  58 /* we can't use cpu_to_be32 outside a function */
  59 #if __BYTE_ORDER == __BIG_ENDIAN
  60 static uint32_t ipv4_rtr_alert = 0x94040000;
  61 #else
  62 static uint32_t ipv4_rtr_alert = 0x00000494;
  63 #endif
  64
  65 static struct {
  66         struct ip6_hbh hdr;
  67         struct ip6_opt_router rt;
  68         uint8_t pad[2];
  69 } ipv6_rtr_alert = {
  70         .hdr = {0, 0},
  71         .rt = {IP6OPT_ROUTER_ALERT, 2, {0, IP6_ALERT_MLD}},
  72         .pad = {0, 0}
  73 };
  74
  75 static struct mrib_iface mifs[MAXMIFS] = {};
  76 static struct uloop_fd mrt_fd = { .fd = -1 };
  77 static struct uloop_fd mrt6_fd = { .fd = -1 };
  78
  79
  80 // Unmap IPv4 address from IPv6
  81 static inline void mrib_unmap(struct in_addr *addr4, const struct in6_addr *addr6)
  82 {
  83         addr4->s_addr = addr6->s6_addr32[3];
  84 }
  85
  86 // Add / delete multicast route
  87 static int mrib_set(const struct in6_addr *group, const struct in6_addr *source,
  88                 struct mrib_iface *iface, mrib_filter dest, bool del)
  89 {
  90         int status = 0;
  91         size_t mifid = iface - mifs;
  92         if (IN6_IS_ADDR_V4MAPPED(group)) {
  93                 struct mfcctl ctl = { .mfcc_parent = mifid };
  94                 mrib_unmap(&ctl.mfcc_origin, source);
  95                 mrib_unmap(&ctl.mfcc_mcastgrp, group);
  96
  97                 if(!del)
  98                         for (size_t i = 0; i < MAXMIFS; ++i)
  99                                 if (dest & (1 << i))
 100                                         ctl.mfcc_ttls[i] = 1;
 101
 102                 if (setsockopt(mrt_fd.fd, IPPROTO_IP,
 103                                 (del) ? MRT_DEL_MFC : MRT_ADD_MFC,
 104                                 &ctl, sizeof(ctl)))
 105                         status = -errno;
 106         } else {
 107                 struct mf6cctl ctl = {
 108                         .mf6cc_origin = {AF_INET6, 0, 0, *source, 0},
 109                         .mf6cc_mcastgrp = {AF_INET6, 0, 0, *group, 0},
 110                         .mf6cc_parent = mifid,
 111                 };
 112
 113                 if(!del)
 114                         for (size_t i = 0; i < MAXMIFS; ++i)
 115                                 if (dest & (1 << i))
 116                                         IF_SET(i, &ctl.mf6cc_ifset);
 117
 118                 if (setsockopt(mrt6_fd.fd, IPPROTO_IPV6,
 119                                 (del) ? MRT6_DEL_MFC : MRT6_ADD_MFC,
 120                                 &ctl, sizeof(ctl)))
 121                         status = -errno;
 122         }
 123
 124         char groupbuf[INET6_ADDRSTRLEN], sourcebuf[INET6_ADDRSTRLEN];
 125         inet_ntop(AF_INET6, group, groupbuf, sizeof(groupbuf));
 126         inet_ntop(AF_INET6, source, sourcebuf, sizeof(sourcebuf));
 127         if(del) {
 128                 L_DEBUG("%s: deleting MFC-entry for %s from %s%%%d: %s",
 129                                 __FUNCTION__, groupbuf, sourcebuf, iface->ifindex, strerror(-status));
 130         } else {
 131                 int ifbuf_len = 0;
 132                 char ifbuf[256] = {0};
 133                 for (size_t i = 0; i < MAXMIFS; ++i)
 134                         if (dest & (1 << i))
 135                                 ifbuf_len += snprintf(&ifbuf[ifbuf_len], sizeof(ifbuf) - ifbuf_len, " %d", mifs[i].ifindex);
 136
 137
 138                 L_DEBUG("%s: setting MFC-entry for %s from %s%%%d to%s: %s",
 139                                 __FUNCTION__, groupbuf, sourcebuf, iface->ifindex, ifbuf, strerror(-status));
 140         }
 141
 142         return status;
 143 }
 144
 145
 146 // We have no way of knowing when a source disappears, so we delete multicast routes from time to time
 147 static void mrib_clean(struct uloop_timeout *t)
 148 {
 149         struct mrib_iface *iface = container_of(t, struct mrib_iface, timer);
 150         omgp_time_t now = omgp_time();
 151         uloop_timeout_cancel(t);
 152
 153         struct mrib_route *c, *n;
 154         list_for_each_entry_safe(c, n, &iface->routes, head) {
 155                 if (c->valid_until <= now || (list_empty(&iface->users) && list_empty(&iface->queriers))) {
 156                         mrib_set(&c->group, &c->source, iface, 0, 1);
 157                         list_del(&c->head);
 158                         free(c);
 159                 } else {
 160                         uloop_timeout_set(t, c->valid_until - now);
 161                         break;
 162                 }
 163         }
 164 }
 165
 166
 167 // Find MIFID by ifindex
 168 static size_t mrib_find(int ifindex)
 169 {
 170         size_t i = 0;
 171         while (i < MAXMIFS && mifs[i].ifindex != ifindex)
 172                 ++i;
 173         return i;
 174 }
 175
 176 // Notify all users of a new multicast source
 177 static void mrib_notify_newsource(struct mrib_iface *iface,
 178                 const struct in6_addr *group, const struct in6_addr *source)
 179 {
 180         mrib_filter filter = 0;
 181         struct mrib_user *user;
 182         list_for_each_entry(user, &iface->users, head)
 183                 if (user->cb_newsource)
 184                         user->cb_newsource(user, group, source, &filter);
 185
 186         char groupbuf[INET6_ADDRSTRLEN], sourcebuf[INET6_ADDRSTRLEN];
 187         inet_ntop(AF_INET6, group, groupbuf, sizeof(groupbuf));
 188         inet_ntop(AF_INET6, source, sourcebuf, sizeof(sourcebuf));
 189         L_DEBUG("%s: detected new multicast source %s for %s on %d",
 190                         __FUNCTION__, sourcebuf, groupbuf, iface->ifindex);
 191
 192         struct mrib_route *route = malloc(sizeof(*route));
 193         if (route) {
 194                 route->group = *group;
 195                 route->source = *source;
 196                 route->valid_until = omgp_time() + MRIB_DEFAULT_LIFETIME * OMGP_TIME_PER_SECOND;
 197
 198                 if (list_empty(&iface->routes))
 199                         uloop_timeout_set(&iface->timer, MRIB_DEFAULT_LIFETIME * OMGP_TIME_PER_SECOND);
 200
 201                 list_add_tail(&route->head, &iface->routes);
 202                 mrib_set(group, source, iface, filter, 0);
 203         }
 204 }
 205
 206 // Calculate IGMP-checksum
 207 static uint16_t igmp_checksum(const uint16_t *buf, size_t len)
 208 {
 209         int32_t sum = 0;
 210
 211         while (len > 1) {
 212                 sum += *buf++;
 213                 sum = (sum + (sum >> 16)) & 0xffff;
 214                 len -= 2;
 215         }
 216
 217         if (len == 1) {
 218                 sum += *((uint8_t*)buf);
 219                 sum += (sum + (sum >> 16)) & 0xffff;
 220         }
 221
 222         return ~sum;
 223 }
 224
 225 // Receive and handle MRT event
 226 static void mrib_receive_mrt(struct uloop_fd *fd, __unused unsigned flags)
 227 {
 228         uint8_t buf[9216], cbuf[CMSG_SPACE(sizeof(struct in_pktinfo))];
 229         char addrbuf[INET_ADDRSTRLEN];
 230         struct sockaddr_in from;
 231
 232         while (true) {
 233                 struct iovec iov = {buf, sizeof(buf)};
 234                 struct msghdr hdr = {
 235                                 .msg_name = (void*)&from,
 236                                 .msg_namelen = sizeof(from),
 237                                 .msg_iov = &iov,
 238                                 .msg_iovlen = 1,
 239                                 .msg_control = cbuf,
 240                                 .msg_controllen = sizeof(cbuf)
 241                 };
 242
 243                 ssize_t len = recvmsg(fd->fd, &hdr, MSG_DONTWAIT);
 244                 if (len < 0 && errno == EAGAIN)
 245                         break;
 246
 247                 struct iphdr *iph = iov.iov_base;
 248                 if (len < (ssize_t)sizeof(*iph))
 249                         continue;
 250
 251                 if (iph->protocol == 0) {
 252                         // Pseudo IP/IGMP-packet from kernel MC-API
 253                         struct igmpmsg *msg = iov.iov_base;
 254                         struct mrib_iface *iface = NULL;
 255                         if (msg->im_vif < MAXMIFS)
 256                                 iface = &mifs[msg->im_vif];
 257
 258                         if (!iface) {
 259                                 L_WARN("MRT kernel-message for unknown MIF %i", msg->im_vif);
 260                                 continue;
 261                         }
 262
 263                         if (msg->im_msgtype != IGMPMSG_NOCACHE) {
 264                                 L_WARN("Unknown MRT kernel-message %i on interface %d",
 265                                                 msg->im_msgtype, iface->ifindex);
 266                                 continue;
 267                         }
 268
 269                         struct in6_addr dst = IN6ADDR_ANY_INIT;
 270                         struct in6_addr src = IN6ADDR_ANY_INIT;
 271                         dst.s6_addr32[2] = cpu_to_be32(0xffff);
 272                         dst.s6_addr32[3] = msg->im_dst.s_addr;
 273                         src.s6_addr32[2] = cpu_to_be32(0xffff);
 274                         src.s6_addr32[3] = msg->im_src.s_addr;
 275
 276                         mrib_notify_newsource(iface, &dst, &src);
 277                 } else {
 278                         // IGMP packet
 279                         if ((len -= iph->ihl * 4) < 0)
 280                                 continue;
 281
 282                         int ifindex = 0;
 283                         for (struct cmsghdr *ch = CMSG_FIRSTHDR(&hdr); ch != NULL; ch = CMSG_NXTHDR(&hdr, ch)) {
 284                                 if (ch->cmsg_level == IPPROTO_IP && ch->cmsg_type == IP_PKTINFO) {
 285                                         struct in_pktinfo *info = (struct in_pktinfo*)CMSG_DATA(ch);
 286                                         ifindex = info->ipi_ifindex;
 287                                 }
 288                         }
 289
 290                         if (ifindex == 0)
 291                                 continue;
 292
 293                         inet_ntop(AF_INET, &from.sin_addr, addrbuf, sizeof(addrbuf));
 294                         struct igmphdr *igmp = (struct igmphdr*)&buf[iph->ihl * 4];
 295
 296                         uint16_t checksum = igmp->csum;
 297                         igmp->csum = 0;
 298
 299                         if (iph->ttl != 1 || len < (ssize_t)sizeof(*igmp) ||
 300                                         checksum != igmp_checksum((uint16_t*)igmp, len)) {
 301                                 L_WARN("%s: ignoring invalid IGMP-message of type %x from %s on %d",
 302                                                 __FUNCTION__, igmp->type, addrbuf, ifindex);
 303                                 continue;
 304                         }
 305
 306                         uint32_t *opts = (uint32_t*)&iph[1];
 307                         bool alert = (void*)&opts[1] <= (void*)igmp && *opts == ipv4_rtr_alert;
 308                         if (!alert && (igmp->type != IGMP_HOST_MEMBERSHIP_QUERY ||
 309                                                         (size_t)len > sizeof(*igmp) || igmp->code > 0)) {
 310                                 L_WARN("%s: ignoring invalid IGMP-message of type %x from %s on %d",
 311                                                 __FUNCTION__, igmp->type, addrbuf, ifindex);
 312                                 continue;
 313                         }
 314
 315                         ssize_t mifid = mrib_find(ifindex);
 316                         if (mifid < MAXMIFS) {
 317                                 struct mrib_querier *q;
 318                                 list_for_each_entry(q, &mifs[mifid].queriers, head)
 319                                         if (q->cb_igmp)
 320                                                 q->cb_igmp(q, igmp, len, &from);
 321                         }
 322                 }
 323         }
 324 }
 325
 326 // Receive and handle MRT6 event
 327 static void mrib_receive_mrt6(struct uloop_fd *fd, __unused unsigned flags)
 328 {
 329         uint8_t buf[9216], cbuf[128];
 330         char addrbuf[INET6_ADDRSTRLEN];
 331         struct sockaddr_in6 from;
 332
 333         while (true) {
 334                 struct iovec iov = {buf, sizeof(buf)};
 335                 struct msghdr hdr = {
 336                                 .msg_name = (void*)&from,
 337                                 .msg_namelen = sizeof(from),
 338                                 .msg_iov = &iov,
 339                                 .msg_iovlen = 1,
 340                                 .msg_control = cbuf,
 341                                 .msg_controllen = sizeof(cbuf)
 342                 };
 343
 344                 ssize_t len = recvmsg(fd->fd, &hdr, MSG_DONTWAIT);
 345                 if (len < 0 && errno == EAGAIN)
 346                         break;
 347
 348                 struct mld_hdr *mld = iov.iov_base;
 349                 if (len < (ssize_t)sizeof(*mld))
 350                         continue;
 351
 352                 if (mld->mld_icmp6_hdr.icmp6_type == 0) {
 353                         // Pseudo ICMPv6/MLD-packet from kernel MC-API
 354                         struct mrt6msg *msg = iov.iov_base;
 355                         struct mrib_iface *iface = NULL;
 356                         if (msg->im6_mif < MAXMIFS)
 357                                 iface = &mifs[msg->im6_mif];
 358
 359                         if (!iface) {
 360                                 L_WARN("MRT6 kernel-message for unknown MIF %i", msg->im6_mif);
 361                                 continue;
 362                         }
 363
 364                         if (msg->im6_msgtype != MRT6MSG_NOCACHE) {
 365                                 L_WARN("Unknown MRT6 kernel-message %i on interface %d",
 366                                                 msg->im6_msgtype, iface->ifindex);
 367                                 continue;
 368                         }
 369
 370                         mrib_notify_newsource(iface, &msg->im6_dst, &msg->im6_src);
 371                 } else {
 372                         int hlim = 0, ifindex = from.sin6_scope_id;
 373                         bool alert = false;
 374                         for (struct cmsghdr *ch = CMSG_FIRSTHDR(&hdr); ch != NULL; ch = CMSG_NXTHDR(&hdr, ch)) {
 375                                 if (ch->cmsg_level == IPPROTO_IPV6 && ch->cmsg_type == IPV6_HOPLIMIT)
 376                                         memcpy(&hlim, CMSG_DATA(ch), sizeof(hlim));
 377                                 else if (ch->cmsg_level == IPPROTO_IPV6 && ch->cmsg_type == IPV6_HOPOPTS &&
 378                                                 ch->cmsg_len >= CMSG_LEN(sizeof(ipv6_rtr_alert)) &&
 379                                                 memmem(CMSG_DATA(ch), ch->cmsg_len - CMSG_LEN(0),
 380                                                                 &ipv6_rtr_alert.rt, sizeof(ipv6_rtr_alert.rt)))
 381                                         alert = true; // FIXME: memmem is wrong
 382                         }
 383                         inet_ntop(AF_INET6, &from.sin6_addr, addrbuf, sizeof(addrbuf));
 384
 385                         if (!IN6_IS_ADDR_LINKLOCAL(&from.sin6_addr) || hlim != 1 || len < 24 || !alert) {
 386                                 L_WARN("mld: ignoring invalid MLD-message of type %d from %s on %d",
 387                                                 mld->mld_icmp6_hdr.icmp6_type, addrbuf, ifindex);
 388                                 continue;
 389                         }
 390
 391                         ssize_t mifid = mrib_find(from.sin6_scope_id);
 392                         if (mifid < MAXMIFS) {
 393                                 struct mrib_querier *q;
 394                                 list_for_each_entry(q, &mifs[mifid].queriers, head)
 395                                         if (q->cb_mld)
 396                                                 q->cb_mld(q, mld, len, &from);
 397                         }
 398                 }
 399         }
 400 }
 401
 402 // Send an IGMP-packet
 403 int mrib_send_igmp(struct mrib_querier *q, struct igmpv3_query *igmp, size_t len,
 404                 const struct sockaddr_in *dest)
 405 {
 406         uint8_t cbuf[CMSG_SPACE(sizeof(struct in_pktinfo))] = {0};
 407         struct iovec iov = {igmp, len};
 408         struct msghdr msg = {
 409                         .msg_name = (void*)dest,
 410                         .msg_namelen = sizeof(*dest),
 411                         .msg_iov = &iov,
 412                         .msg_iovlen = 1,
 413                         .msg_control = cbuf,
 414                         .msg_controllen = sizeof(cbuf)
 415         };
 416
 417         igmp->csum = 0;
 418         igmp->csum = igmp_checksum((uint16_t*)igmp, len);
 419
 420         // Set control data (define destination interface)
 421         struct cmsghdr *chdr = CMSG_FIRSTHDR(&msg);
 422         chdr->cmsg_level = IPPROTO_IP;
 423         chdr->cmsg_type = IP_PKTINFO;
 424         chdr->cmsg_len = CMSG_LEN(sizeof(struct in_pktinfo));
 425
 426         struct in_pktinfo *pktinfo = (struct in_pktinfo*)CMSG_DATA(chdr);
 427         pktinfo->ipi_addr.s_addr = 0;
 428         pktinfo->ipi_ifindex = q->iface->ifindex;
 429         if (mrib_igmp_source(q, &pktinfo->ipi_spec_dst))
 430                 return -errno;
 431
 432         ssize_t s = sendmsg(mrt_fd.fd, &msg, MSG_DONTWAIT);
 433         return (s < 0) ? -errno : (s < (ssize_t)len) ? -EMSGSIZE : 0;
 434 }
 435
 436 // Send an IGMP-packet
 437 int mrib_send_mld(struct mrib_querier *q, struct mld_hdr *mld, size_t len,
 438                 const struct sockaddr_in6 *dest)
 439 {
 440         uint8_t cbuf[CMSG_SPACE(sizeof(struct in6_pktinfo))] = {0};
 441         struct iovec iov = {mld, len};
 442         struct msghdr msg = {
 443                         .msg_name = (void*)dest,
 444                         .msg_namelen = sizeof(*dest),
 445                         .msg_iov = &iov,
 446                         .msg_iovlen = 1,
 447                         .msg_control = cbuf,
 448                         .msg_controllen = sizeof(cbuf)
 449         };
 450
 451         // Set control data (define destination interface)
 452         struct cmsghdr *chdr = CMSG_FIRSTHDR(&msg);
 453         chdr->cmsg_level = IPPROTO_IPV6;
 454         chdr->cmsg_type = IPV6_PKTINFO;
 455         chdr->cmsg_len = CMSG_LEN(sizeof(struct in6_pktinfo));
 456
 457         struct in6_pktinfo *pktinfo = (struct in6_pktinfo*)CMSG_DATA(chdr);
 458         pktinfo->ipi6_ifindex = q->iface->ifindex;
 459         if (mrib_mld_source(q, &pktinfo->ipi6_addr))
 460                 return -errno;
 461
 462         ssize_t s = sendmsg(mrt6_fd.fd, &msg, MSG_DONTWAIT);
 463         return (s < 0) ? -errno : (s < (ssize_t)len) ? -EMSGSIZE : 0;
 464 }
 465
 466 // Initialize MRIB
 467 static int mrib_init(void)
 468 {
 469         int fd;
 470         int val;
 471
 472         if ((fd = socket(AF_INET, SOCK_RAW, IPPROTO_IGMP)) < 0)
 473                 goto err;
 474
 475         val = 1;
 476         if (setsockopt(fd, IPPROTO_IP, MRT_INIT, &val, sizeof(val)))
 477                 goto err;
 478
 479         if (setsockopt(fd, IPPROTO_IP, IP_PKTINFO, &val, sizeof(val)))
 480                 goto err;
 481
 482         // Configure IP header fields
 483         if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_TTL, &val, sizeof(val)))
 484                 goto err;
 485
 486         val = 0xc0;
 487         if (setsockopt(fd, IPPROTO_IP, IP_TOS, &val, sizeof(val)))
 488                 goto err;
 489
 490         val = 0;
 491         if (setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, &val, sizeof(val)))
 492                 goto err;
 493
 494         // Set router-alert option
 495         if (setsockopt(fd, IPPROTO_IP, IP_OPTIONS, &ipv4_rtr_alert, sizeof(ipv4_rtr_alert)))
 496                 goto err;
 497
 498         mrt_fd.fd = fd;
 499
 500
 501         if ((fd = socket(AF_INET6, SOCK_RAW, IPPROTO_ICMPV6)) < 0)
 502                 goto err;
 503
 504         // We need to know the source interface and hop-opts
 505         val = 1;
 506         if (setsockopt(fd, IPPROTO_IPV6, MRT6_INIT, &val, sizeof(val)))
 507                 goto err;
 508
 509         if (setsockopt(fd, IPPROTO_IPV6, IPV6_RECVHOPOPTS, &val, sizeof(val)))
 510                 goto err;
 511
 512         if (setsockopt(fd, IPPROTO_IPV6, IPV6_RECVHOPLIMIT, &val, sizeof(val)))
 513                 goto err;
 514
 515         // MLD has hoplimit 1
 516         if (setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_HOPS, &val, sizeof(val)))
 517                 goto err;
 518
 519         val = 0;
 520         if (setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &val, sizeof(val)))
 521                 goto err;
 522
 523         // Let the kernel compute our checksums
 524         val = 2;
 525         if (setsockopt(fd, IPPROTO_RAW, IPV6_CHECKSUM, &val, sizeof(val)))
 526                 goto err;
 527
 528         // Set hop-by-hop router alert on outgoing
 529         if (setsockopt(fd, IPPROTO_IPV6, IPV6_HOPOPTS, &ipv6_rtr_alert, sizeof(ipv6_rtr_alert)))
 530                 goto err;
 531
 532         // Set ICMP6 filter
 533         struct icmp6_filter flt;
 534         ICMP6_FILTER_SETBLOCKALL(&flt);
 535         ICMP6_FILTER_SETPASS(ICMPV6_MGM_QUERY, &flt);
 536         ICMP6_FILTER_SETPASS(ICMPV6_MGM_REPORT, &flt);
 537         ICMP6_FILTER_SETPASS(ICMPV6_MGM_REDUCTION, &flt);
 538         ICMP6_FILTER_SETPASS(ICMPV6_MLD2_REPORT, &flt);
 539         if (setsockopt(fd, IPPROTO_ICMPV6, ICMP6_FILTER, &flt, sizeof(flt)))
 540                 goto err;
 541
 542         mrt6_fd.fd = fd;
 543
 544         mrt_fd.cb = mrib_receive_mrt;
 545         mrt6_fd.cb = mrib_receive_mrt6;
 546
 547         uloop_fd_add(&mrt_fd, ULOOP_READ | ULOOP_EDGE_TRIGGER);
 548         uloop_fd_add(&mrt6_fd, ULOOP_READ | ULOOP_EDGE_TRIGGER);
 549
 550         fd = -1;
 551         errno = 0;
 552
 553 err:
 554         if (fd >= 0)
 555                 close(fd);
 556         return -errno;
 557 }
 558
 559 // Create new interface entry
 560 static struct mrib_iface* mrib_get_iface(int ifindex)
 561 {
 562         if (mrt_fd.fd < 0 && mrib_init() < 0)
 563                 return NULL;
 564
 565         size_t mifid = mrib_find(ifindex);
 566         if (mifid < MAXMIFS)
 567                 return &mifs[mifid];
 568
 569         errno = EBUSY;
 570         if ((mifid = mrib_find(0)) >= MAXMIFS)
 571                 return NULL;
 572
 573         struct mrib_iface *iface = &mifs[mifid];
 574
 575         struct vifctl ctl = {mifid, VIFF_USE_IFINDEX, 1, 0, { .vifc_lcl_ifindex = ifindex }, {INADDR_ANY}};
 576         if (setsockopt(mrt_fd.fd, IPPROTO_IP, MRT_ADD_VIF, &ctl, sizeof(ctl)))
 577                 return NULL;
 578
 579         struct mif6ctl ctl6 = {mifid, 0, 1, ifindex, 0};
 580         if (setsockopt(mrt6_fd.fd, IPPROTO_IPV6, MRT6_ADD_MIF, &ctl6, sizeof(ctl6)))
 581                 return NULL;
 582
 583         struct ip_mreqn mreq = {{INADDR_ALLIGMPV3RTRS_GROUP}, {INADDR_ANY}, ifindex};
 584         setsockopt(mrt_fd.fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
 585
 586         mreq.imr_multiaddr.s_addr = cpu_to_be32(INADDR_ALLRTRS_GROUP);
 587         setsockopt(mrt_fd.fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
 588
 589         struct ipv6_mreq mreq6 = {MLD2_ALL_MCR_INIT, ifindex};
 590         setsockopt(mrt6_fd.fd, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &mreq6, sizeof(mreq6));
 591
 592         mreq6.ipv6mr_multiaddr.s6_addr[15] = 0x02;
 593         setsockopt(mrt6_fd.fd, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &mreq6, sizeof(mreq6));
 594
 595         iface->timer.cb = mrib_clean;
 596         iface->ifindex = ifindex;
 597         INIT_LIST_HEAD(&iface->routes);
 598         INIT_LIST_HEAD(&iface->users);
 599         INIT_LIST_HEAD(&iface->queriers);
 600         return iface;
 601 }
 602
 603 // Remove interfaces if it has no more users
 604 static void mrib_clean_iface(struct mrib_iface *iface)
 605 {
 606         if (list_empty(&iface->users) && list_empty(&iface->queriers)) {
 607                 iface->ifindex = 0;
 608                 mrib_clean(&iface->timer);
 609
 610                 size_t mifid = iface - mifs;
 611                 struct vifctl ctl = {mifid, VIFF_USE_IFINDEX, 1, 0,
 612                                 { .vifc_lcl_ifindex = iface->ifindex }, {INADDR_ANY}};
 613                 setsockopt(mrt_fd.fd, IPPROTO_IP, MRT_DEL_VIF, &ctl, sizeof(ctl));
 614
 615                 struct mif6ctl ctl6 = {mifid, 0, 1, iface->ifindex, 0};
 616                 setsockopt(mrt6_fd.fd, IPPROTO_IPV6, MRT6_DEL_MIF, &ctl6, sizeof(ctl6));
 617
 618                 struct ip_mreqn mreq = {{INADDR_ALLIGMPV3RTRS_GROUP}, {INADDR_ANY}, iface->ifindex};
 619                 setsockopt(mrt_fd.fd, IPPROTO_IP, IP_DROP_MEMBERSHIP, &mreq, sizeof(mreq));
 620
 621                 mreq.imr_multiaddr.s_addr = cpu_to_be32(INADDR_ALLRTRS_GROUP);
 622                 setsockopt(mrt_fd.fd, IPPROTO_IP, IP_DROP_MEMBERSHIP, &mreq, sizeof(mreq));
 623
 624                 struct ipv6_mreq mreq6 = {MLD2_ALL_MCR_INIT, iface->ifindex};
 625                 setsockopt(mrt6_fd.fd, IPPROTO_IPV6, IPV6_DROP_MEMBERSHIP, &mreq6, sizeof(mreq6));
 626
 627                 mreq6.ipv6mr_multiaddr.s6_addr[15] = 0x02;
 628                 setsockopt(mrt6_fd.fd, IPPROTO_IPV6, IPV6_DROP_MEMBERSHIP, &mreq6, sizeof(mreq6));
 629         }
 630 }
 631
 632 // Register a new interface to mrib
 633 int mrib_attach_user(struct mrib_user *user, int ifindex, mrib_cb *cb_newsource)
 634 {
 635         struct mrib_iface *iface = mrib_get_iface(ifindex);
 636         if (!iface)
 637                 return -errno;
 638
 639         if (user->iface == iface)
 640                 return -EALREADY;
 641
 642         list_add(&user->head, &iface->users);
 643         user->iface = iface;
 644         user->cb_newsource = cb_newsource;
 645         return 0;
 646 }
 647
 648 // Deregister an interface from mrib
 649 void mrib_detach_user(struct mrib_user *user)
 650 {
 651         struct mrib_iface *iface = user->iface;
 652         if (!iface)
 653                 return;
 654
 655         user->iface = NULL;
 656         list_del(&user->head);
 657         mrib_clean_iface(iface);
 658 }
 659
 660 // Register a querier to mrib
 661 int mrib_attach_querier(struct mrib_querier *querier, int ifindex, mrib_igmp_cb *cb_igmp, mrib_mld_cb *cb_mld)
 662 {
 663         struct mrib_iface *iface = mrib_get_iface(ifindex);
 664         if (!iface)
 665                 return -errno;
 666
 667         list_add(&querier->head, &iface->queriers);
 668         querier->iface = iface;
 669         querier->cb_igmp = cb_igmp;
 670         querier->cb_mld = cb_mld;
 671         return 0;
 672 }
 673
 674 // Deregister a querier from mrib
 675 void mrib_detach_querier(struct mrib_querier *querier)
 676 {
 677         struct mrib_iface *iface = querier->iface;
 678         if (!iface)
 679                 return;
 680
 681         querier->iface = NULL;
 682         list_del(&querier->head);
 683         mrib_clean_iface(iface);
 684 }
 685
 686 static uint8_t prefix_contains(const struct in6_addr *p, uint8_t plen, const struct in6_addr *addr)
 687 {
 688         int blen = plen >> 3;
 689         if(blen && memcmp(p, addr, blen))
 690                 return 0;
 691
 692         int rem = plen & 0x07;
 693         if(rem && ((p->s6_addr[blen] ^ addr->s6_addr[blen]) >> (8 - rem)))
 694                 return 0;
 695
 696         return 1;
 697 }
 698
 699 // Flush state for a multicast route
 700 int mrib_flush(struct mrib_user *user, const struct in6_addr *group, uint8_t group_plen, const struct in6_addr *source)
 701 {
 702         struct mrib_iface *iface = user->iface;
 703         if (!iface)
 704                 return -ENOENT;
 705
 706         bool found = false;
 707         struct mrib_route *route, *n;
 708         list_for_each_entry_safe(route, n, &iface->routes, head) {
 709                 if (prefix_contains(group, group_plen, &route->group) &&
 710                                 (!source || IN6_ARE_ADDR_EQUAL(&route->source, source))) {
 711                         route->valid_until = 0;
 712                         list_del(&route->head);
 713                         list_add(&route->head, &iface->routes);
 714                         found = true;
 715                 }
 716         }
 717
 718         if (found)
 719                 mrib_clean(&iface->timer);
 720
 721         return (found) ? 0 : -ENOENT;
 722 }
 723
 724 // Add an interface to the filter
 725 int mrib_filter_add(mrib_filter *filter, struct mrib_user *user)
 726 {
 727         struct mrib_iface *iface = user->iface;
 728         if (!iface)
 729                 return -ENOENT;
 730
 731         *filter |= 1 << (iface - mifs);
 732         return 0;
 733 }
 734
 735 // Get MLD source address
 736 int mrib_mld_source(struct mrib_querier *q, struct in6_addr *source)
 737 {
 738         struct sockaddr_in6 addr = {AF_INET6, 0, 0, MLD2_ALL_MCR_INIT, q->iface->ifindex};
 739         socklen_t alen = sizeof(addr);
 740         int sock = socket(AF_INET6, SOCK_RAW | SOCK_CLOEXEC, IPPROTO_ICMPV6);
 741         int ret = 0;
 742
 743         if (sock < 0 || connect(sock, (struct sockaddr*)&addr, sizeof(addr)))
 744                 ret = -errno;
 745
 746         if (ret || getsockname(sock, (struct sockaddr*)&addr, &alen)) {
 747                 L_WARN("%s: failed to detect local source address on %d", __FUNCTION__, q->iface->ifindex);
 748                 ret = -errno;
 749         }
 750
 751         close(sock);
 752
 753         if (ret == 0)
 754                 *source = addr.sin6_addr;
 755
 756         return ret;
 757 }
 758
 759
 760 // Get IGMP source address
 761 int mrib_igmp_source(struct mrib_querier *q, struct in_addr *source)
 762 {
 763         struct sockaddr_in addr = {AF_INET, 0, {cpu_to_be32(INADDR_ALLHOSTS_GROUP)}, {0}};
 764         socklen_t alen = sizeof(addr);
 765         struct ifreq ifr = {.ifr_name = ""};
 766         int sock = socket(AF_INET, SOCK_RAW | SOCK_CLOEXEC, IPPROTO_IGMP);
 767         int ret = 0;
 768
 769         ifr.ifr_ifindex = q->iface->ifindex;
 770
 771         if (sock < 0 || ioctl(sock, SIOCGIFNAME, &ifr) ||
 772                         setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, ifr.ifr_name, strlen(ifr.ifr_name)))
 773                 ret = -errno;
 774
 775
 776         if (ret || connect(sock, (struct sockaddr*)&addr, sizeof(addr)))
 777                 ret = -errno;
 778
 779         if (ret || getsockname(sock, (struct sockaddr*)&addr, &alen)) {
 780                 L_WARN("%s: failed to detect local source address on %d", __FUNCTION__, q->iface->ifindex);
 781                 ret = -errno;
 782         }
 783
 784         close(sock);
 785
 786         if (ret == 0)
 787                 *source = addr.sin_addr;
 788
 789         return ret;
 790 }