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