Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 1 | /****************************************************************************** |
| 2 | * Copyright 2016 Foxconn |
| 3 | * |
| 4 | * Licensed under the Apache License, Version 2.0 (the "License"); |
| 5 | * you may not use this file except in compliance with the License. |
| 6 | * You may obtain a copy of the License at |
| 7 | * |
| 8 | * http://www.apache.org/licenses/LICENSE-2.0 |
| 9 | * |
| 10 | * Unless required by applicable law or agreed to in writing, software |
| 11 | * distributed under the License is distributed on an "AS IS" BASIS, |
| 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 13 | * See the License for the specific language governing permissions and |
| 14 | * limitations under the License. |
| 15 | ******************************************************************************/ |
Jeremy Kerr | 982b7bc | 2016-02-25 10:54:39 +0800 | [diff] [blame] | 16 | |
| 17 | #include <stdio.h> |
| 18 | #include <stdlib.h> |
| 19 | #include <string.h> |
| 20 | #include <errno.h> |
| 21 | #include <time.h> |
| 22 | #include <unistd.h> |
Jeremy Kerr | d786533 | 2016-02-25 14:52:50 +0800 | [diff] [blame] | 23 | #include <err.h> |
Jeremy Kerr | 982b7bc | 2016-02-25 10:54:39 +0800 | [diff] [blame] | 24 | |
| 25 | #include <sys/ioctl.h> |
| 26 | #include <sys/socket.h> |
| 27 | |
| 28 | #include <netinet/in.h> |
Jeremy Kerr | 760d6ac | 2016-02-25 14:45:33 +0800 | [diff] [blame] | 29 | #include <arpa/inet.h> |
Jeremy Kerr | 982b7bc | 2016-02-25 10:54:39 +0800 | [diff] [blame] | 30 | |
| 31 | #include <linux/if_packet.h> |
| 32 | #include <linux/if_ether.h> |
| 33 | #include <linux/if_arp.h> |
| 34 | |
Jeremy Kerr | 760d6ac | 2016-02-25 14:45:33 +0800 | [diff] [blame] | 35 | struct arp_packet { |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 36 | struct ethhdr eh; |
| 37 | struct arphdr arp; |
Jeremy Kerr | 760d6ac | 2016-02-25 14:45:33 +0800 | [diff] [blame] | 38 | uint8_t src_mac[ETH_ALEN]; |
| 39 | struct in_addr src_ip; |
| 40 | uint8_t dest_mac[ETH_ALEN]; |
| 41 | struct in_addr dest_ip; |
| 42 | } __attribute__((packed)); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 43 | |
Jeremy Kerr | 7b3bcf7 | 2016-02-25 14:29:55 +0800 | [diff] [blame] | 44 | static int send_arp_packet(int fd, |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 45 | int ifindex, |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 46 | unsigned char *src_mac, |
Jeremy Kerr | 760d6ac | 2016-02-25 14:45:33 +0800 | [diff] [blame] | 47 | struct in_addr *src_ip, |
| 48 | unsigned char *dest_mac, |
| 49 | struct in_addr *dest_ip) |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 50 | { |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 51 | struct sockaddr_ll socket_address; |
Jeremy Kerr | 5bb869d | 2016-02-25 18:47:08 +0800 | [diff] [blame] | 52 | struct arp_packet arp; |
Jeremy Kerr | d786533 | 2016-02-25 14:52:50 +0800 | [diff] [blame] | 53 | int rc; |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 54 | |
Jeremy Kerr | 5bb869d | 2016-02-25 18:47:08 +0800 | [diff] [blame] | 55 | memset(&arp, 0, sizeof(arp)); |
| 56 | |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 57 | /* Prepare our link-layer address: raw packet interface, |
| 58 | * using the ifindex interface, receiving ARP packets |
| 59 | */ |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 60 | socket_address.sll_family = PF_PACKET; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 61 | socket_address.sll_protocol = htons(ETH_P_ARP); |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 62 | socket_address.sll_ifindex = ifindex; |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 63 | socket_address.sll_hatype = ARPHRD_ETHER; |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 64 | socket_address.sll_pkttype = PACKET_OTHERHOST; |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 65 | socket_address.sll_halen = ETH_ALEN; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 66 | memcpy(socket_address.sll_addr, dest_mac, ETH_ALEN); |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 67 | |
| 68 | /* set the frame header */ |
Jeremy Kerr | 5bb869d | 2016-02-25 18:47:08 +0800 | [diff] [blame] | 69 | memcpy(arp.eh.h_dest, (void *)dest_mac, ETH_ALEN); |
| 70 | memcpy(arp.eh.h_source, (void *)src_mac, ETH_ALEN); |
| 71 | arp.eh.h_proto = htons(ETH_P_ARP); |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 72 | |
| 73 | /* Fill InARP request data for ethernet + ipv4 */ |
Jeremy Kerr | 5bb869d | 2016-02-25 18:47:08 +0800 | [diff] [blame] | 74 | arp.arp.ar_hrd = htons(ARPHRD_ETHER); |
| 75 | arp.arp.ar_pro = htons(ETH_P_ARP); |
| 76 | arp.arp.ar_hln = ETH_ALEN; |
| 77 | arp.arp.ar_pln = 4; |
| 78 | arp.arp.ar_op = htons(ARPOP_InREPLY); |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 79 | |
| 80 | /* fill arp ethernet mac & ipv4 info */ |
Jeremy Kerr | 5bb869d | 2016-02-25 18:47:08 +0800 | [diff] [blame] | 81 | memcpy(&arp.src_mac, src_mac, sizeof(arp.src_mac)); |
| 82 | memcpy(&arp.src_ip, src_ip, sizeof(arp.src_ip)); |
| 83 | memcpy(&arp.dest_mac, dest_mac, sizeof(arp.dest_mac)); |
| 84 | memcpy(&arp.dest_ip, dest_ip, sizeof(arp.dest_ip)); |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 85 | |
| 86 | /* send the packet */ |
Jeremy Kerr | 5bb869d | 2016-02-25 18:47:08 +0800 | [diff] [blame] | 87 | rc = sendto(fd, &arp, sizeof(arp), 0, |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 88 | (struct sockaddr *)&socket_address, |
| 89 | sizeof(socket_address)); |
Jeremy Kerr | d786533 | 2016-02-25 14:52:50 +0800 | [diff] [blame] | 90 | if (rc < 0) |
| 91 | warn("failure sending ARP response"); |
| 92 | |
| 93 | return rc; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 94 | } |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 95 | |
Jeremy Kerr | 7b3bcf7 | 2016-02-25 14:29:55 +0800 | [diff] [blame] | 96 | static void show_mac_addr(const char *name, unsigned char *mac_addr) |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 97 | { |
| 98 | int i; |
| 99 | printf("%s MAC address: ", name); |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 100 | for (i = 0; i < 6; i++) { |
| 101 | printf("%.2X%c", (unsigned char)mac_addr[i], |
| 102 | (i == 5) ? '\n' : ':'); |
| 103 | } |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 104 | return; |
| 105 | } |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 106 | |
Jeremy Kerr | e54e483 | 2016-02-25 17:23:10 +0800 | [diff] [blame] | 107 | static int get_local_ipaddr(int fd, const char *ifname, struct in_addr *addr) |
| 108 | { |
| 109 | struct sockaddr_in *sa; |
| 110 | struct ifreq ifreq; |
| 111 | int rc; |
| 112 | |
| 113 | memset(&ifreq, 0, sizeof(ifreq)); |
| 114 | strcpy(ifreq.ifr_name, ifname); |
| 115 | |
| 116 | rc = ioctl(fd, SIOCGIFADDR, &ifreq); |
| 117 | if (rc) { |
| 118 | warn("Error querying local address for %s", ifname); |
| 119 | return -1; |
| 120 | } |
| 121 | |
| 122 | if (ifreq.ifr_addr.sa_family != AF_INET) { |
| 123 | warnx("Unknown address family %d in address response", |
| 124 | ifreq.ifr_addr.sa_family); |
| 125 | return -1; |
| 126 | } |
| 127 | |
| 128 | sa = (struct sockaddr_in *)&ifreq.ifr_addr; |
| 129 | memcpy(addr, &sa->sin_addr, sizeof(*addr)); |
| 130 | return 0; |
| 131 | } |
| 132 | |
Jeremy Kerr | 115522d | 2016-02-25 10:24:14 +0800 | [diff] [blame] | 133 | static void usage(const char *progname) |
| 134 | { |
| 135 | fprintf(stderr, "Usage: %s <interface>\n", progname); |
| 136 | } |
| 137 | |
| 138 | int main(int argc, char **argv) |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 139 | { |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 140 | static struct ifreq ifreq_buffer; |
Jeremy Kerr | 3f8a28e | 2016-02-25 18:56:01 +0800 | [diff] [blame^] | 141 | struct arp_packet inarp_req; |
Jeremy Kerr | 115522d | 2016-02-25 10:24:14 +0800 | [diff] [blame] | 142 | const char *ifname; |
Jeremy Kerr | 3f8a28e | 2016-02-25 18:56:01 +0800 | [diff] [blame^] | 143 | ssize_t len; |
Jeremy Kerr | 760d6ac | 2016-02-25 14:45:33 +0800 | [diff] [blame] | 144 | int fd, ret; |
Jeremy Kerr | 115522d | 2016-02-25 10:24:14 +0800 | [diff] [blame] | 145 | |
| 146 | if (argc < 2) { |
| 147 | usage(argv[0]); |
| 148 | return EXIT_FAILURE; |
| 149 | } |
| 150 | |
| 151 | ifname = argv[1]; |
| 152 | |
Jeremy Kerr | d786533 | 2016-02-25 14:52:50 +0800 | [diff] [blame] | 153 | if (strlen(ifname) > IFNAMSIZ) |
| 154 | errx(EXIT_FAILURE, "Interface name '%s' is invalid", ifname); |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 155 | |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 156 | static unsigned char src_mac[6]; |
Jeremy Kerr | e54e483 | 2016-02-25 17:23:10 +0800 | [diff] [blame] | 157 | static struct in_addr local_ip; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 158 | int ifindex; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 159 | |
Jeremy Kerr | c304ce7 | 2016-02-25 18:37:14 +0800 | [diff] [blame] | 160 | fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ARP)); |
Jeremy Kerr | d786533 | 2016-02-25 14:52:50 +0800 | [diff] [blame] | 161 | if (fd < 0) |
| 162 | err(EXIT_FAILURE, "Error opening ARP socket"); |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 163 | |
| 164 | /* Query local mac address */ |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 165 | memset(&ifreq_buffer, 0x00, sizeof(ifreq_buffer)); |
Jeremy Kerr | 115522d | 2016-02-25 10:24:14 +0800 | [diff] [blame] | 166 | strcpy(ifreq_buffer.ifr_name, ifname); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 167 | ret = ioctl(fd, SIOCGIFHWADDR, &ifreq_buffer); |
Jeremy Kerr | d786533 | 2016-02-25 14:52:50 +0800 | [diff] [blame] | 168 | if (ret < 0) |
| 169 | err(EXIT_FAILURE, "Error querying local MAC address"); |
| 170 | |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 171 | memcpy(src_mac, ifreq_buffer.ifr_hwaddr.sa_data, ETH_ALEN); |
Jeremy Kerr | 115522d | 2016-02-25 10:24:14 +0800 | [diff] [blame] | 172 | show_mac_addr(ifname, src_mac); |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 173 | |
| 174 | /* find the ifindex of the interface we're using */ |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 175 | memset(&ifreq_buffer, 0x00, sizeof(ifreq_buffer)); |
Jeremy Kerr | 115522d | 2016-02-25 10:24:14 +0800 | [diff] [blame] | 176 | strcpy(ifreq_buffer.ifr_name, ifname); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 177 | ret = ioctl(fd, SIOCGIFINDEX, &ifreq_buffer); |
Jeremy Kerr | d786533 | 2016-02-25 14:52:50 +0800 | [diff] [blame] | 178 | if (ret < 0) |
| 179 | err(EXIT_FAILURE, "Error querying interface %s", ifname); |
| 180 | |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 181 | ifindex = ifreq_buffer.ifr_ifindex; |
| 182 | |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 183 | while (1) { |
Jeremy Kerr | 3f8a28e | 2016-02-25 18:56:01 +0800 | [diff] [blame^] | 184 | len = recvfrom(fd, &inarp_req, sizeof(inarp_req), 0, |
| 185 | NULL, NULL); |
| 186 | if (len <= 0) { |
Jeremy Kerr | 77cafea | 2016-02-25 17:15:28 +0800 | [diff] [blame] | 187 | if (errno == EINTR) |
| 188 | continue; |
| 189 | err(EXIT_FAILURE, "Error recieving ARP packet"); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 190 | } |
Jeremy Kerr | 77cafea | 2016-02-25 17:15:28 +0800 | [diff] [blame] | 191 | |
Jeremy Kerr | 3f8a28e | 2016-02-25 18:56:01 +0800 | [diff] [blame^] | 192 | /* Is this packet large enough for an inarp? */ |
| 193 | if ((size_t)len < sizeof(inarp_req)) |
| 194 | continue; |
| 195 | |
| 196 | /* ... is it an inarp request? */ |
| 197 | if (ntohs(inarp_req.arp.ar_op) != ARPOP_InREQUEST) |
Jeremy Kerr | 77cafea | 2016-02-25 17:15:28 +0800 | [diff] [blame] | 198 | continue; |
| 199 | |
| 200 | /* ... for us? */ |
Jeremy Kerr | 3f8a28e | 2016-02-25 18:56:01 +0800 | [diff] [blame^] | 201 | if (memcmp(src_mac, inarp_req.eh.h_dest, ETH_ALEN)) |
Jeremy Kerr | 77cafea | 2016-02-25 17:15:28 +0800 | [diff] [blame] | 202 | continue; |
| 203 | |
| 204 | printf("src mac =%02x:%02x:%02x:%02x:%02x:%02x\n", |
Jeremy Kerr | 3f8a28e | 2016-02-25 18:56:01 +0800 | [diff] [blame^] | 205 | inarp_req.src_mac[0], |
| 206 | inarp_req.src_mac[1], |
| 207 | inarp_req.src_mac[2], |
| 208 | inarp_req.src_mac[3], |
| 209 | inarp_req.src_mac[4], |
| 210 | inarp_req.src_mac[5]); |
Jeremy Kerr | 77cafea | 2016-02-25 17:15:28 +0800 | [diff] [blame] | 211 | |
Jeremy Kerr | 3f8a28e | 2016-02-25 18:56:01 +0800 | [diff] [blame^] | 212 | printf("src ip = %s\n", inet_ntoa(inarp_req.src_ip)); |
Jeremy Kerr | 77cafea | 2016-02-25 17:15:28 +0800 | [diff] [blame] | 213 | |
Jeremy Kerr | e54e483 | 2016-02-25 17:23:10 +0800 | [diff] [blame] | 214 | ret = get_local_ipaddr(fd, ifname, &local_ip); |
| 215 | /* if we don't have a local IP address to send, just drop the |
| 216 | * request */ |
| 217 | if (ret) |
| 218 | continue; |
| 219 | |
Jeremy Kerr | 5bb869d | 2016-02-25 18:47:08 +0800 | [diff] [blame] | 220 | send_arp_packet(fd, ifindex, |
Jeremy Kerr | 3f8a28e | 2016-02-25 18:56:01 +0800 | [diff] [blame^] | 221 | inarp_req.dest_mac, |
Jeremy Kerr | e54e483 | 2016-02-25 17:23:10 +0800 | [diff] [blame] | 222 | &local_ip, |
Jeremy Kerr | 3f8a28e | 2016-02-25 18:56:01 +0800 | [diff] [blame^] | 223 | inarp_req.src_mac, |
| 224 | &inarp_req.src_ip); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 225 | } |
| 226 | close(fd); |
| 227 | return 0; |
| 228 | } |