src/bfd.c

   1 #include <syslog.h>
   2 #include <signal.h>
   3 #include <stddef.h>
   4 #include <stdlib.h>
   5 #include <netinet/ip6.h>
   6 #include <netinet/icmp6.h>
   7
   8 #include <sys/socket.h>
   9 #include <net/if.h>
  10 #include <net/ethernet.h>
  11 #include <netpacket/packet.h>
  12 #include <linux/rtnetlink.h>
  13 #include <linux/filter.h>
  14
  15 #include <errno.h>
  16 #include <fcntl.h>
  17 #include <unistd.h>
  18
  19 #include "odhcp6c.h"
  20
  21 static int sock = -1, rtnl = -1;
  22 static int if_index = -1;
  23 static int bfd_failed = 0, bfd_limit = 0, bfd_interval = 0;
  24 static bool bfd_armed = false;
  25
  26
  27 static void bfd_send(int signal __attribute__((unused)))
  28 {
  29         struct {
  30                 struct ip6_hdr ip6;
  31                 struct icmp6_hdr icmp6;
  32         } ping;
  33         memset(&ping, 0, sizeof(ping));
  34
  35         ping.ip6.ip6_vfc = 6 << 4;
  36         ping.ip6.ip6_plen = htons(8);
  37         ping.ip6.ip6_nxt = IPPROTO_ICMPV6;
  38         ping.ip6.ip6_hlim = 255;
  39
  40         ping.icmp6.icmp6_type = ICMP6_ECHO_REQUEST;
  41         ping.icmp6.icmp6_data32[0] = htonl(0xbfd0bfd);
  42
  43         size_t pdlen, rtlen;
  44         struct odhcp6c_entry *pd = odhcp6c_get_state(STATE_IA_PD, &pdlen), *cpd = NULL;
  45         struct odhcp6c_entry *rt = odhcp6c_get_state(STATE_RA_ROUTE, &rtlen), *crt = NULL;
  46         bool crt_found = false;
  47
  48         alarm(bfd_interval);
  49
  50         if (bfd_armed) {
  51                 if (++bfd_failed > bfd_limit) {
  52                         raise(SIGUSR2);
  53                         return;
  54                 }
  55         }
  56
  57         // Detect PD-Prefix
  58         for (size_t i = 0; i < pdlen / sizeof(*pd); ++i)
  59                 if (!cpd || ((cpd->target.s6_addr[0] & 7) == 0xfc) > ((pd[i].target.s6_addr[0] & 7) == 0xfc)
  60                                 || cpd->preferred < pd[i].preferred)
  61                         cpd = &pd[i];
  62
  63         // Detect default router
  64         for (size_t i = 0; i < rtlen / sizeof(*rt); ++i)
  65                 if (IN6_IS_ADDR_UNSPECIFIED(&rt[i].target) && (!crt || crt->priority > rt[i].priority))
  66                         crt = &rt[i];
  67
  68         struct sockaddr_ll dest = {
  69                 .sll_family = AF_PACKET,
  70                 .sll_protocol = htons(ETH_P_IPV6),
  71                 .sll_ifindex = if_index,
  72                 .sll_halen = ETH_ALEN,
  73         };
  74
  75         if (crt) {
  76                 struct {
  77                         struct nlmsghdr hdr;
  78                         struct ndmsg ndm;
  79                 } req = {
  80                         .hdr = {sizeof(req), RTM_GETNEIGH, NLM_F_REQUEST | NLM_F_DUMP, 1, 0},
  81                         .ndm = {.ndm_family = AF_INET6, .ndm_ifindex = if_index}
  82                 };
  83                 send(rtnl, &req, sizeof(req), 0);
  84
  85                 uint8_t buf[8192];
  86                 struct nlmsghdr *nhm;
  87                 do {
  88                         ssize_t read = recv(rtnl, buf, sizeof(buf), 0);
  89                         nhm = (struct nlmsghdr*)buf;
  90                         if ((read < 0 && errno == EINTR) || !NLMSG_OK(nhm, (size_t)read))
  91                                 continue;
  92                         else if (read < 0)
  93                                 break;
  94
  95                         for (; read > 0 && NLMSG_OK(nhm, (size_t)read); nhm = NLMSG_NEXT(nhm, read)) {
  96                                 ssize_t attrlen = NLMSG_PAYLOAD(nhm, sizeof(struct ndmsg));
  97                                 if (nhm->nlmsg_type != RTM_NEWNEIGH || attrlen <= 0) {
  98                                         nhm = NULL;
  99                                         break;
 100                                 }
 101
 102                                 // Already have our MAC
 103                                 if (crt_found)
 104                                         continue;
 105
 106                                 struct ndmsg *ndm = NLMSG_DATA(nhm);
 107                                 for (struct rtattr *rta = (struct rtattr*)&ndm[1];
 108                                                 attrlen > 0 && RTA_OK(rta, (size_t)attrlen);
 109                                                 rta = RTA_NEXT(rta, attrlen)) {
 110                                         if (rta->rta_type == NDA_DST) {
 111                                                 crt_found = IN6_ARE_ADDR_EQUAL(RTA_DATA(rta), &crt->router);
 112                                         } else if (rta->rta_type == NDA_LLADDR) {
 113                                                 memcpy(dest.sll_addr, RTA_DATA(rta), ETH_ALEN);
 114                                         }
 115                                 }
 116                         }
 117                 } while (nhm);
 118         }
 119
 120         if (!crt_found || !cpd)
 121                 return;
 122
 123         ping.ip6.ip6_src = cpd->target;
 124         ping.ip6.ip6_dst = cpd->target;
 125
 126         struct sock_filter bpf[] = {
 127                 BPF_STMT(BPF_LD | BPF_W | BPF_ABS, offsetof(struct ip6_hdr, ip6_plen)),
 128                 BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, 8 << 16 | IPPROTO_ICMPV6 << 8 | 254, 0, 13),
 129                 BPF_STMT(BPF_LD | BPF_W | BPF_ABS, offsetof(struct ip6_hdr, ip6_dst)),
 130                 BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, ntohl(ping.ip6.ip6_dst.s6_addr32[0]), 0, 11),
 131                 BPF_STMT(BPF_LD | BPF_W | BPF_ABS, offsetof(struct ip6_hdr, ip6_dst) + 4),
 132                 BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, ntohl(ping.ip6.ip6_dst.s6_addr32[1]), 0, 9),
 133                 BPF_STMT(BPF_LD | BPF_W | BPF_ABS, offsetof(struct ip6_hdr, ip6_dst) + 8),
 134                 BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, ntohl(ping.ip6.ip6_dst.s6_addr32[2]), 0, 7),
 135                 BPF_STMT(BPF_LD | BPF_W | BPF_ABS, offsetof(struct ip6_hdr, ip6_dst) + 12),
 136                 BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, ntohl(ping.ip6.ip6_dst.s6_addr32[3]), 0, 5),
 137                 BPF_STMT(BPF_LD | BPF_W | BPF_ABS, sizeof(struct ip6_hdr) +
 138                                 offsetof(struct icmp6_hdr, icmp6_type)),
 139                 BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, ICMP6_ECHO_REQUEST << 24, 0, 3),
 140                 BPF_STMT(BPF_LD | BPF_W | BPF_ABS, sizeof(struct ip6_hdr) +
 141                                 offsetof(struct icmp6_hdr, icmp6_data32)),
 142                 BPF_JUMP(BPF_JMP | BPF_JEQ | BPF_K, ntohl(ping.icmp6.icmp6_data32[0]), 0, 1),
 143                 BPF_STMT(BPF_RET | BPF_K, 0xffffffff),
 144                 BPF_STMT(BPF_RET | BPF_K, 0),
 145         };
 146         struct sock_fprog bpf_prog = {sizeof(bpf) / sizeof(*bpf), bpf};
 147
 148
 149         if (sock < 0) {
 150                 sock = socket(AF_PACKET, SOCK_DGRAM | SOCK_CLOEXEC, htons(ETH_P_IPV6));
 151                 bind(sock, (struct sockaddr*)&dest, sizeof(dest));
 152
 153                 fcntl(sock, F_SETOWN, getpid());
 154                 fcntl(sock, F_SETFL, fcntl(sock, F_GETFL) | O_ASYNC);
 155         }
 156
 157         setsockopt(sock, SOL_SOCKET, SO_DETACH_FILTER, &bpf_prog, sizeof(bpf_prog));
 158         if (setsockopt(sock, SOL_SOCKET, SO_ATTACH_FILTER, &bpf_prog, sizeof(bpf_prog))) {
 159                 close(sock);
 160                 sock = -1;
 161                 return;
 162         }
 163
 164         uint8_t dummy[8];
 165         while (recv(sock, dummy, sizeof(dummy), MSG_DONTWAIT | MSG_TRUNC) > 0);
 166
 167         sendto(sock, &ping, sizeof(ping), MSG_DONTWAIT,
 168                         (struct sockaddr*)&dest, sizeof(dest));
 169 }
 170
 171
 172 void bfd_receive(void)
 173 {
 174         uint8_t dummy[8];
 175         while (recv(sock, dummy, sizeof(dummy), MSG_DONTWAIT | MSG_TRUNC) > 0) {
 176                 bfd_failed = 0;
 177                 bfd_armed = true;
 178         }
 179 }
 180
 181
 182 int bfd_start(const char *ifname, int limit, int interval)
 183 {
 184         if_index = if_nametoindex(ifname);
 185         bfd_armed = false;
 186         bfd_failed = 0;
 187         bfd_limit = limit;
 188         bfd_interval = interval;
 189
 190         if (limit < 1 || interval < 1)
 191                 return 0;
 192
 193         rtnl = socket(AF_NETLINK, SOCK_DGRAM | SOCK_CLOEXEC, NETLINK_ROUTE);
 194         struct sockaddr_nl rtnl_kernel = { .nl_family = AF_NETLINK };
 195         connect(rtnl, (const struct sockaddr*)&rtnl_kernel, sizeof(rtnl_kernel));
 196
 197         signal(SIGALRM, bfd_send);
 198         alarm(5);
 199         return 0;
 200 }
 201
 202
 203 void bfd_stop(void)
 204 {
 205         alarm(0);
 206         close(sock);
 207         close(rtnl);
 208
 209         sock = -1;
 210         rtnl = -1;
 211 }