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