5 #include <netinet/ip6.h>
6 #include <netinet/icmp6.h>
8 #include <sys/socket.h>
10 #include <net/ethernet.h>
11 #include <netpacket/packet.h>
12 #include <linux/rtnetlink.h>
13 #include <linux/filter.h>
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;
27 static void bfd_send(int signal
__attribute__((unused
)))
31 struct icmp6_hdr icmp6
;
33 memset(&ping
, 0, sizeof(ping
));
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;
40 ping
.icmp6
.icmp6_type
= ICMP6_ECHO_REQUEST
;
41 ping
.icmp6
.icmp6_data32
[0] = htonl(0xbfd0bfd);
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;
51 if (++bfd_failed
> bfd_limit
) {
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
)
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
))
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
,
80 .hdr
= {sizeof(req
), RTM_GETNEIGH
, NLM_F_REQUEST
| NLM_F_DUMP
, 1, 0},
81 .ndm
= {.ndm_family
= AF_INET6
, .ndm_ifindex
= if_index
}
83 send(rtnl
, &req
, sizeof(req
), 0);
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
))
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) {
102 // Already have our MAC
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
);
120 if (!crt_found
|| !cpd
)
123 ping
.ip6
.ip6_src
= cpd
->target
;
124 ping
.ip6
.ip6_dst
= cpd
->target
;
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),
146 struct sock_fprog bpf_prog
= {sizeof(bpf
) / sizeof(*bpf
), bpf
};
150 sock
= socket(AF_PACKET
, SOCK_DGRAM
| SOCK_CLOEXEC
, htons(ETH_P_IPV6
));
151 bind(sock
, (struct sockaddr
*)&dest
, sizeof(dest
));
153 fcntl(sock
, F_SETOWN
, getpid());
154 fcntl(sock
, F_SETFL
, fcntl(sock
, F_GETFL
) | O_ASYNC
);
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
))) {
165 while (recv(sock
, dummy
, sizeof(dummy
), MSG_DONTWAIT
| MSG_TRUNC
) > 0);
167 sendto(sock
, &ping
, sizeof(ping
), MSG_DONTWAIT
,
168 (struct sockaddr
*)&dest
, sizeof(dest
));
172 void bfd_receive(void)
175 while (recv(sock
, dummy
, sizeof(dummy
), MSG_DONTWAIT
| MSG_TRUNC
) > 0) {
182 int bfd_start(const char *ifname
, int limit
, int interval
)
184 if_index
= if_nametoindex(ifname
);
188 bfd_interval
= interval
;
190 if (limit
< 1 || interval
< 1)
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
));
197 signal(SIGALRM
, bfd_send
);