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