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> |
| 23 | |
| 24 | #include <sys/ioctl.h> |
| 25 | #include <sys/socket.h> |
| 26 | |
| 27 | #include <netinet/in.h> |
| 28 | |
| 29 | #include <linux/if_packet.h> |
| 30 | #include <linux/if_ether.h> |
| 31 | #include <linux/if_arp.h> |
| 32 | |
Jeremy Kerr | 66a2805 | 2016-02-25 14:31:35 +0800 | [diff] [blame^] | 33 | #define ETH_ARP_FRAME_LEN ( \ |
| 34 | sizeof(struct ethhdr) + \ |
| 35 | sizeof(struct arphdr) + \ |
| 36 | ((ETH_ALEN + 4) * 2)) |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 37 | |
| 38 | struct ARP_DATA { |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 39 | unsigned char src_mac[ETH_ALEN]; |
| 40 | unsigned char src_ip[4]; |
| 41 | unsigned char dest_mac[ETH_ALEN]; |
| 42 | unsigned char dest_ip[4]; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 43 | }; |
| 44 | struct ETH_ARP_PACKET { |
| 45 | struct ethhdr eh; |
| 46 | struct arphdr arp; |
| 47 | struct ARP_DATA arp_data; |
| 48 | }; |
| 49 | |
| 50 | struct ETH_ARP_PACKET *inarp_req; |
| 51 | |
Jeremy Kerr | 7b3bcf7 | 2016-02-25 14:29:55 +0800 | [diff] [blame] | 52 | static int send_arp_packet(int fd, |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 53 | int ifindex, |
| 54 | struct ETH_ARP_PACKET *eth_arp, |
| 55 | __be16 ar_op, |
| 56 | unsigned char *src_mac, |
| 57 | unsigned char *src_ip, |
| 58 | unsigned char *dest_mac, unsigned char *dest_ip) |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 59 | { |
| 60 | int send_result = 0; |
| 61 | struct ethhdr *eh = ð_arp->eh; |
| 62 | struct arphdr *arp = ð_arp->arp; |
| 63 | struct ARP_DATA *arp_data = ð_arp->arp_data; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 64 | struct sockaddr_ll socket_address; |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 65 | |
| 66 | /* Prepare our link-layer address: raw packet interface, |
| 67 | * using the ifindex interface, receiving ARP packets |
| 68 | */ |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 69 | socket_address.sll_family = PF_PACKET; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 70 | socket_address.sll_protocol = htons(ETH_P_ARP); |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 71 | socket_address.sll_ifindex = ifindex; |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 72 | socket_address.sll_hatype = ARPHRD_ETHER; |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 73 | socket_address.sll_pkttype = PACKET_OTHERHOST; |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 74 | socket_address.sll_halen = ETH_ALEN; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 75 | memcpy(socket_address.sll_addr, dest_mac, ETH_ALEN); |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 76 | |
| 77 | /* set the frame header */ |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 78 | memcpy((void *)eh->h_dest, (void *)dest_mac, ETH_ALEN); |
| 79 | memcpy((void *)eh->h_source, (void *)src_mac, ETH_ALEN); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 80 | eh->h_proto = htons(ETH_P_ARP); |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 81 | |
| 82 | /* Fill InARP request data for ethernet + ipv4 */ |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 83 | arp->ar_hrd = htons(ARPHRD_ETHER); |
| 84 | arp->ar_pro = htons(ETH_P_ARP); |
| 85 | arp->ar_hln = ETH_ALEN; |
| 86 | arp->ar_pln = 4; |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 87 | arp->ar_op = htons(ar_op); |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 88 | |
| 89 | /* fill arp ethernet mac & ipv4 info */ |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 90 | arp_data = (void *)(arp + 1); |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 91 | memcpy(arp_data->src_mac, (void *)src_mac, ETH_ALEN); |
| 92 | memcpy(arp_data->src_ip, src_ip, 4); |
| 93 | memcpy(arp_data->dest_mac, (void *)dest_mac, ETH_ALEN); |
| 94 | memcpy(arp_data->dest_ip, dest_ip, 4); |
| 95 | |
| 96 | /* send the packet */ |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 97 | send_result = sendto(fd, eth_arp, ETH_ARP_FRAME_LEN, 0, |
| 98 | (struct sockaddr *)&socket_address, |
| 99 | sizeof(socket_address)); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 100 | if (send_result == -1) { |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 101 | printf("sendto: [%s]\n", strerror(errno)); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 102 | } |
| 103 | return send_result; |
| 104 | } |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 105 | |
Jeremy Kerr | 7b3bcf7 | 2016-02-25 14:29:55 +0800 | [diff] [blame] | 106 | static void show_mac_addr(const char *name, unsigned char *mac_addr) |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 107 | { |
| 108 | int i; |
| 109 | printf("%s MAC address: ", name); |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 110 | for (i = 0; i < 6; i++) { |
| 111 | printf("%.2X%c", (unsigned char)mac_addr[i], |
| 112 | (i == 5) ? '\n' : ':'); |
| 113 | } |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 114 | return; |
| 115 | } |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 116 | |
Jeremy Kerr | 115522d | 2016-02-25 10:24:14 +0800 | [diff] [blame] | 117 | static void usage(const char *progname) |
| 118 | { |
| 119 | fprintf(stderr, "Usage: %s <interface>\n", progname); |
| 120 | } |
| 121 | |
| 122 | int main(int argc, char **argv) |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 123 | { |
| 124 | int fd, ret; |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 125 | /*buffer for ethernet frame */ |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 126 | static unsigned char buffer[ETH_FRAME_LEN]; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 127 | int send_result = 0; |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 128 | static struct ifreq ifreq_buffer; |
Jeremy Kerr | 115522d | 2016-02-25 10:24:14 +0800 | [diff] [blame] | 129 | const char *ifname; |
| 130 | |
| 131 | if (argc < 2) { |
| 132 | usage(argv[0]); |
| 133 | return EXIT_FAILURE; |
| 134 | } |
| 135 | |
| 136 | ifname = argv[1]; |
| 137 | |
| 138 | if (strlen(ifname) > IFNAMSIZ) { |
| 139 | fprintf(stderr, "Interface name '%s' is invalid\n", ifname); |
| 140 | return EXIT_FAILURE; |
| 141 | } |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 142 | |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 143 | static unsigned char src_mac[6]; |
| 144 | static unsigned char src_ip[4]; |
| 145 | int ifindex; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 146 | |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 147 | if (((fd = socket(AF_PACKET, SOCK_PACKET, htons(ETH_P_ARP)))) == -1) { |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 148 | printf("socket: [%s]\n", strerror(errno)); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 149 | exit(-1); |
| 150 | } |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 151 | |
| 152 | /* Query local mac address */ |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 153 | memset(&ifreq_buffer, 0x00, sizeof(ifreq_buffer)); |
Jeremy Kerr | 115522d | 2016-02-25 10:24:14 +0800 | [diff] [blame] | 154 | strcpy(ifreq_buffer.ifr_name, ifname); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 155 | ret = ioctl(fd, SIOCGIFHWADDR, &ifreq_buffer); |
| 156 | if (ret == -1) { |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 157 | printf("ioctl2: [%s]\n", strerror(errno)); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 158 | close(fd); |
| 159 | exit(-1); |
| 160 | } |
| 161 | memcpy(src_mac, ifreq_buffer.ifr_hwaddr.sa_data, ETH_ALEN); |
Jeremy Kerr | 115522d | 2016-02-25 10:24:14 +0800 | [diff] [blame] | 162 | show_mac_addr(ifname, src_mac); |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 163 | |
| 164 | /* find the ifindex of the interface we're using */ |
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, SIOCGIFINDEX, &ifreq_buffer); |
| 168 | if (ret == -1) { |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 169 | printf("ioctl4: [%s]\n", strerror(errno)); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 170 | close(fd); |
| 171 | exit(-1); |
| 172 | } |
| 173 | ifindex = ifreq_buffer.ifr_ifindex; |
| 174 | |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 175 | /* length of the received frame */ |
| 176 | int length = 0; |
| 177 | static struct ETH_ARP_PACKET *inarp_req = |
| 178 | (struct ETH_ARP_PACKET *)buffer; |
| 179 | static struct ETH_ARP_PACKET inarp_resp; |
| 180 | |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 181 | while (1) { |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 182 | /* get local ip address */ |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 183 | memset(&ifreq_buffer, 0x00, sizeof(ifreq_buffer)); |
Jeremy Kerr | 115522d | 2016-02-25 10:24:14 +0800 | [diff] [blame] | 184 | strcpy(ifreq_buffer.ifr_name, ifname); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 185 | ret = ioctl(fd, SIOCGIFADDR, &ifreq_buffer); |
| 186 | if (ret == -1) { |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 187 | sleep(1); |
| 188 | continue; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 189 | } |
| 190 | |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 191 | if (AF_INET == ifreq_buffer.ifr_addr.sa_family) { |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 192 | memcpy(src_ip, &ifreq_buffer.ifr_addr.sa_data[2], 4); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 193 | } else { |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 194 | printf("unknown address family [%d]!\n", |
| 195 | ifreq_buffer.ifr_addr.sa_family); |
| 196 | sleep(1); |
| 197 | continue; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 198 | } |
| 199 | |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 200 | memset((void *)&inarp_resp, 0, sizeof inarp_resp); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 201 | length = recvfrom(fd, buffer, ETH_ARP_FRAME_LEN, 0, NULL, NULL); |
| 202 | if (length == -1) { |
| 203 | sleep(1); |
| 204 | } |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 205 | if (0 == memcmp(src_mac, inarp_req->eh.h_dest, ETH_ALEN)) { |
Jeremy Kerr | c10bedb | 2016-02-25 14:26:38 +0800 | [diff] [blame] | 206 | if (ntohs(inarp_req->arp.ar_op) == ARPOP_InREQUEST) { |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 207 | |
| 208 | printf |
| 209 | ("src mac =%02x:%02x:%02x:%02x:%02x:%02x\r\n", |
| 210 | inarp_req->arp_data.src_mac[0], |
| 211 | inarp_req->arp_data.src_mac[1], |
| 212 | inarp_req->arp_data.src_mac[2], |
| 213 | inarp_req->arp_data.src_mac[3], |
| 214 | inarp_req->arp_data.src_mac[4], |
| 215 | inarp_req->arp_data.src_mac[5] |
| 216 | ); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 217 | printf("src ip =%d:%d:%d:%d\r\n", |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 218 | inarp_req->arp_data.src_ip[0], |
| 219 | inarp_req->arp_data.src_ip[1], |
| 220 | inarp_req->arp_data.src_ip[2], |
| 221 | inarp_req->arp_data.src_ip[3] |
| 222 | ); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 223 | int fd_1; |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 224 | if (((fd_1 = |
| 225 | socket(AF_PACKET, SOCK_RAW, 0))) == -1) { |
| 226 | printf("socket: [%s]\n", |
| 227 | strerror(errno)); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 228 | exit(-1); |
| 229 | } |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 230 | send_result = |
| 231 | send_arp_packet(fd_1, ifindex, &inarp_resp, |
| 232 | ARPOP_InREPLY, |
| 233 | inarp_req->arp_data. |
| 234 | dest_mac, src_ip, |
| 235 | inarp_req->arp_data.src_mac, |
| 236 | inarp_req->arp_data.src_ip); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 237 | close(fd_1); |
| 238 | if (send_result == -1) { |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 239 | printf("[Rsp] sendto: [%s]\n", |
| 240 | strerror(errno)); |
| 241 | sleep(1); |
| 242 | continue; |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 243 | } |
| 244 | } |
Jeremy Kerr | 5514f7b | 2016-02-25 12:41:44 +0800 | [diff] [blame] | 245 | memset(buffer, 0, sizeof(buffer)); |
Jeremy Kerr | 42dc98c | 2016-02-25 10:23:14 +0800 | [diff] [blame] | 246 | } |
| 247 | } |
| 248 | close(fd); |
| 249 | return 0; |
| 250 | } |