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