1 /****************************************************************************** 2 * Copyright 2016 Foxconn 3 * Copyright 2016 IBM Corporation 4 * 5 * Licensed under the Apache License, Version 2.0 (the "License"); 6 * you may not use this file except in compliance with the License. 7 * You may obtain a copy of the License at 8 * 9 * http://www.apache.org/licenses/LICENSE-2.0 10 * 11 * Unless required by applicable law or agreed to in writing, software 12 * distributed under the License is distributed on an "AS IS" BASIS, 13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 * See the License for the specific language governing permissions and 15 * limitations under the License. 16 ******************************************************************************/ 17 18 #include <err.h> 19 #include <errno.h> 20 #include <stdio.h> 21 #include <stdbool.h> 22 #include <stdlib.h> 23 #include <string.h> 24 #include <time.h> 25 #include <unistd.h> 26 27 #include <sys/ioctl.h> 28 #include <sys/poll.h> 29 #include <sys/socket.h> 30 31 #include <arpa/inet.h> 32 #include <netinet/in.h> 33 34 #include <linux/if_arp.h> 35 #include <linux/if_ether.h> 36 #include <linux/if_link.h> 37 #include <linux/if_packet.h> 38 #include <linux/netlink.h> 39 #include <linux/rtnetlink.h> 40 41 struct eth_addr { 42 uint8_t eth_addr[ETH_ALEN]; 43 } __attribute__((packed)); 44 45 struct arp_packet { 46 struct ethhdr eh; 47 struct arphdr arp; 48 struct eth_addr src_mac; 49 struct in_addr src_ip; 50 struct eth_addr dest_mac; 51 struct in_addr dest_ip; 52 } __attribute__((packed)); 53 54 struct interface { 55 int ifindex; 56 char ifname[IFNAMSIZ+1]; 57 struct eth_addr eth_addr; 58 }; 59 60 struct inarp_ctx { 61 int arp_sd; 62 int nl_sd; 63 struct interface *interfaces; 64 unsigned int n_interfaces; 65 }; 66 67 /* helpers for rtnetlink message iteration */ 68 #define for_each_nlmsg(buf, nlmsg, len) \ 69 for (nlmsg = (struct nlmsghdr *)buf; \ 70 NLMSG_OK(nlmsg, len) && nlmsg->nlmsg_type != NLMSG_DONE; \ 71 nlmsg = NLMSG_NEXT(nlmsg, len)) 72 73 #define for_each_rta(buf, rta, attrlen) \ 74 for (rta = (struct rtattr *)(buf); RTA_OK(rta, attrlen); \ 75 rta = RTA_NEXT(rta, attrlen)) 76 77 static int send_arp_packet(int fd, 78 int ifindex, 79 const struct eth_addr *src_mac, 80 const struct in_addr *src_ip, 81 const struct eth_addr *dest_mac, 82 const struct in_addr *dest_ip) 83 { 84 struct sockaddr_ll addr; 85 struct arp_packet arp; 86 int rc; 87 88 memset(&arp, 0, sizeof(arp)); 89 90 /* Prepare our link-layer address: raw packet interface, 91 * using the ifindex interface, receiving ARP packets 92 */ 93 addr.sll_family = PF_PACKET; 94 addr.sll_protocol = htons(ETH_P_ARP); 95 addr.sll_ifindex = ifindex; 96 addr.sll_hatype = ARPHRD_ETHER; 97 addr.sll_pkttype = PACKET_OTHERHOST; 98 addr.sll_halen = ETH_ALEN; 99 memcpy(addr.sll_addr, dest_mac, ETH_ALEN); 100 101 /* set the frame header */ 102 memcpy(arp.eh.h_dest, dest_mac, ETH_ALEN); 103 memcpy(arp.eh.h_source, src_mac, ETH_ALEN); 104 arp.eh.h_proto = htons(ETH_P_ARP); 105 106 /* Fill InARP request data for ethernet + ipv4 */ 107 arp.arp.ar_hrd = htons(ARPHRD_ETHER); 108 arp.arp.ar_pro = htons(ETH_P_ARP); 109 arp.arp.ar_hln = ETH_ALEN; 110 arp.arp.ar_pln = 4; 111 arp.arp.ar_op = htons(ARPOP_InREPLY); 112 113 /* fill arp ethernet mac & ipv4 info */ 114 memcpy(&arp.src_mac, src_mac, sizeof(arp.src_mac)); 115 memcpy(&arp.src_ip, src_ip, sizeof(arp.src_ip)); 116 memcpy(&arp.dest_mac, dest_mac, sizeof(arp.dest_mac)); 117 memcpy(&arp.dest_ip, dest_ip, sizeof(arp.dest_ip)); 118 119 /* send the packet */ 120 rc = sendto(fd, &arp, sizeof(arp), 0, 121 (struct sockaddr *)&addr, sizeof(addr)); 122 if (rc < 0) 123 warn("failure sending ARP response"); 124 125 return rc; 126 } 127 128 static const char *eth_mac_to_str(const struct eth_addr *mac_addr) 129 { 130 static char mac_str[ETH_ALEN * (sizeof("00:") - 1)]; 131 const uint8_t *addr = mac_addr->eth_addr; 132 133 snprintf(mac_str, sizeof(mac_str), 134 "%02x:%02x:%02x:%02x:%02x:%02x", 135 addr[0], addr[1], addr[2], 136 addr[3], addr[4], addr[5]); 137 138 return mac_str; 139 } 140 141 static int do_ifreq(int fd, unsigned long type, 142 const char *ifname, struct ifreq *ifreq) 143 { 144 memset(ifreq, 0, sizeof(*ifreq)); 145 strncpy(ifreq->ifr_name, ifname, sizeof(ifreq->ifr_name)); 146 147 return ioctl(fd, type, ifreq); 148 } 149 150 static int get_local_ipaddr(int fd, const char *ifname, struct in_addr *addr) 151 { 152 struct sockaddr_in *sa; 153 struct ifreq ifreq; 154 int rc; 155 156 rc = do_ifreq(fd, SIOCGIFADDR, ifname, &ifreq); 157 if (rc) { 158 warn("Error querying local IP address for %s", ifname); 159 return -1; 160 } 161 162 if (ifreq.ifr_addr.sa_family != AF_INET) { 163 warnx("Unknown address family %d in address response", 164 ifreq.ifr_addr.sa_family); 165 return -1; 166 } 167 168 sa = (struct sockaddr_in *)&ifreq.ifr_addr; 169 memcpy(addr, &sa->sin_addr, sizeof(*addr)); 170 return 0; 171 } 172 173 static struct interface *find_interface_by_ifindex(struct inarp_ctx *inarp, 174 int ifindex) 175 { 176 unsigned int i; 177 178 for (i = 0; i < inarp->n_interfaces; i++) { 179 struct interface *iface = &inarp->interfaces[i]; 180 if (iface->ifindex == ifindex) 181 return iface; 182 } 183 184 return NULL; 185 } 186 187 static int init_netlink(struct inarp_ctx *inarp) 188 { 189 struct sockaddr_nl addr; 190 int rc; 191 struct { 192 struct nlmsghdr nlmsg; 193 struct rtgenmsg rtmsg; 194 } msg; 195 196 /* create our socket to listen for rtnetlink events */ 197 inarp->nl_sd = socket(AF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE); 198 if (inarp->nl_sd < 0) { 199 warn("Error opening netlink socket"); 200 return -1; 201 } 202 203 memset(&addr, 0, sizeof(addr)); 204 addr.nl_family = AF_NETLINK; 205 addr.nl_groups = RTMGRP_LINK; 206 207 rc = bind(inarp->nl_sd, (struct sockaddr *)&addr, sizeof(addr)); 208 if (rc) { 209 warn("Error binding to netlink address"); 210 goto err_close; 211 } 212 213 /* send a query for current interfaces */ 214 memset(&msg, 0, sizeof(msg)); 215 216 msg.nlmsg.nlmsg_len = sizeof(msg); 217 msg.nlmsg.nlmsg_type = RTM_GETLINK; 218 msg.nlmsg.nlmsg_flags = NLM_F_REQUEST | NLM_F_ROOT; 219 msg.rtmsg.rtgen_family = AF_UNSPEC; 220 221 rc = send(inarp->nl_sd, &msg, sizeof(msg), MSG_NOSIGNAL); 222 if (rc != sizeof(msg)) { 223 warn("Failed to query current links"); 224 goto err_close; 225 } 226 227 return 0; 228 229 err_close: 230 close(inarp->nl_sd); 231 return -1; 232 } 233 234 static void netlink_nlmsg_dellink(struct inarp_ctx *inarp, 235 struct interface *iface) 236 { 237 int i; 238 239 if (!iface) 240 return; 241 242 printf("dropping interface: %s, [%s]\n", iface->ifname, 243 eth_mac_to_str(&iface->eth_addr)); 244 245 /* find the index of the array element to remove */ 246 i = iface - inarp->interfaces; 247 248 /* remove interface from our array */ 249 inarp->n_interfaces--; 250 inarp->interfaces = realloc(inarp->interfaces, 251 inarp->n_interfaces * sizeof(*iface)); 252 memmove(iface, iface + 1, 253 sizeof(*iface) * (inarp->n_interfaces - i)); 254 255 } 256 static void netlink_nlmsg_newlink(struct inarp_ctx *inarp, 257 struct interface *iface, struct ifinfomsg *ifmsg, int len) 258 { 259 struct rtattr *attr; 260 bool new = false; 261 262 /* 263 * We shouldn't already have an interface for this ifindex; so create 264 * one. If we do, we'll update the hwaddr and name to the new values. 265 */ 266 if (!iface) { 267 inarp->n_interfaces++; 268 inarp->interfaces = realloc(inarp->interfaces, 269 inarp->n_interfaces * sizeof(*iface)); 270 iface = &inarp->interfaces[inarp->n_interfaces-1]; 271 new = true; 272 } 273 274 memset(iface, 0, sizeof(*iface)); 275 iface->ifindex = ifmsg->ifi_index; 276 277 for_each_rta(ifmsg + 1, attr, len) { 278 void *data = RTA_DATA(attr); 279 280 switch (attr->rta_type) { 281 case IFLA_ADDRESS: 282 memcpy(&iface->eth_addr.eth_addr, data, 283 sizeof(iface->eth_addr.eth_addr)); 284 break; 285 286 case IFLA_IFNAME: 287 strncpy(iface->ifname, data, IFNAMSIZ); 288 break; 289 } 290 } 291 292 printf("%s interface: %s, [%s]\n", 293 new ? "adding" : "updating", 294 iface->ifname, 295 eth_mac_to_str(&iface->eth_addr)); 296 fflush(stdout); 297 } 298 299 static void netlink_nlmsg(struct inarp_ctx *inarp, struct nlmsghdr *nlmsg) 300 { 301 struct ifinfomsg *ifmsg; 302 struct interface *iface; 303 int len; 304 305 len = nlmsg->nlmsg_len - sizeof(*ifmsg); 306 ifmsg = NLMSG_DATA(nlmsg); 307 308 iface = find_interface_by_ifindex(inarp, ifmsg->ifi_index); 309 310 switch (nlmsg->nlmsg_type) { 311 case RTM_DELLINK: 312 netlink_nlmsg_dellink(inarp, iface); 313 break; 314 case RTM_NEWLINK: 315 netlink_nlmsg_newlink(inarp, iface, ifmsg, len); 316 break; 317 default: 318 break; 319 } 320 } 321 322 static void netlink_recv(struct inarp_ctx *inarp) 323 { 324 struct nlmsghdr *nlmsg; 325 uint8_t buf[16384]; 326 int len; 327 328 len = recv(inarp->nl_sd, &buf, sizeof(buf), 0); 329 if (len < 0) { 330 warn("Error receiving netlink msg"); 331 return; 332 } 333 334 for_each_nlmsg(buf, nlmsg, len) 335 netlink_nlmsg(inarp, nlmsg); 336 } 337 338 static void arp_recv(struct inarp_ctx *inarp) 339 { 340 struct arp_packet inarp_req; 341 struct sockaddr_ll addr; 342 struct in_addr local_ip; 343 struct interface *iface; 344 socklen_t addrlen; 345 int len, rc; 346 347 addrlen = sizeof(addr); 348 len = recvfrom(inarp->arp_sd, &inarp_req, 349 sizeof(inarp_req), 0, 350 (struct sockaddr *)&addr, &addrlen); 351 if (len <= 0) { 352 if (errno == EINTR) 353 return; 354 err(EXIT_FAILURE, "Error recieving ARP packet"); 355 } 356 357 /* 358 * struct sockaddr_ll allows for 8 bytes of hardware address; 359 * we only need ETH_ALEN for a full ethernet address. 360 */ 361 if (addrlen < sizeof(addr) - (8 - ETH_ALEN)) 362 return; 363 364 if (addr.sll_family != AF_PACKET) 365 return; 366 367 iface = find_interface_by_ifindex(inarp, addr.sll_ifindex); 368 if (!iface) 369 return; 370 371 /* Is this packet large enough for an inarp? */ 372 if ((size_t)len < sizeof(inarp_req)) 373 return; 374 375 /* ... is it an inarp request? */ 376 if (ntohs(inarp_req.arp.ar_op) != ARPOP_InREQUEST) 377 return; 378 379 /* ... for us? */ 380 if (memcmp(&iface->eth_addr, inarp_req.eh.h_dest, ETH_ALEN)) 381 return; 382 383 printf("src mac: %s\n", eth_mac_to_str(&inarp_req.src_mac)); 384 printf("src ip: %s\n", inet_ntoa(inarp_req.src_ip)); 385 386 rc = get_local_ipaddr(inarp->arp_sd, iface->ifname, 387 &local_ip); 388 /* if we don't have a local IP address to send, just drop the 389 * request */ 390 if (rc) 391 return; 392 393 printf("local mac: %s\n", eth_mac_to_str(&iface->eth_addr)); 394 printf("local ip: %s\n", inet_ntoa(local_ip)); 395 396 send_arp_packet(inarp->arp_sd, iface->ifindex, 397 &inarp_req.dest_mac, 398 &local_ip, 399 &inarp_req.src_mac, 400 &inarp_req.src_ip); 401 } 402 403 int main(void) 404 { 405 struct inarp_ctx inarp; 406 int ret; 407 408 memset(&inarp, 0, sizeof(inarp)); 409 410 inarp.arp_sd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ARP)); 411 if (inarp.arp_sd < 0) 412 err(EXIT_FAILURE, "Error opening ARP socket"); 413 414 ret = init_netlink(&inarp); 415 if (ret) 416 exit(EXIT_FAILURE); 417 418 while (1) { 419 struct pollfd pollfds[2]; 420 421 pollfds[0].fd = inarp.arp_sd; 422 pollfds[0].events = POLLIN; 423 pollfds[1].fd = inarp.nl_sd; 424 pollfds[1].events = POLLIN; 425 426 ret = poll(pollfds, 2, -1); 427 if (ret < 0) 428 err(EXIT_FAILURE, "Poll failed"); 429 430 if (pollfds[0].revents) 431 arp_recv(&inarp); 432 433 if (pollfds[1].revents) 434 netlink_recv(&inarp); 435 436 437 } 438 close(inarp.arp_sd); 439 close(inarp.nl_sd); 440 return 0; 441 } 442