blob: 3e8ba2c45e1aa2d58a63d2505777abb9cfdc3460 [file] [log] [blame]
Jeremy Kerr42dc98c2016-02-25 10:23:14 +08001/******************************************************************************
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 Kerr982b7bc2016-02-25 10:54:39 +080016
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 Kerrd7865332016-02-25 14:52:50 +080023#include <err.h>
Jeremy Kerr982b7bc2016-02-25 10:54:39 +080024
25#include <sys/ioctl.h>
26#include <sys/socket.h>
27
28#include <netinet/in.h>
Jeremy Kerr760d6ac2016-02-25 14:45:33 +080029#include <arpa/inet.h>
Jeremy Kerr982b7bc2016-02-25 10:54:39 +080030
31#include <linux/if_packet.h>
32#include <linux/if_ether.h>
33#include <linux/if_arp.h>
34
Jeremy Kerr66a28052016-02-25 14:31:35 +080035#define ETH_ARP_FRAME_LEN ( \
36 sizeof(struct ethhdr) + \
37 sizeof(struct arphdr) + \
Jeremy Kerr760d6ac2016-02-25 14:45:33 +080038 ((ETH_ALEN + sizeof(struct in_addr)) * 2))
Jeremy Kerr42dc98c2016-02-25 10:23:14 +080039
Jeremy Kerr760d6ac2016-02-25 14:45:33 +080040struct arp_packet {
Jeremy Kerr42dc98c2016-02-25 10:23:14 +080041 struct ethhdr eh;
42 struct arphdr arp;
Jeremy Kerr760d6ac2016-02-25 14:45:33 +080043 uint8_t src_mac[ETH_ALEN];
44 struct in_addr src_ip;
45 uint8_t dest_mac[ETH_ALEN];
46 struct in_addr dest_ip;
47} __attribute__((packed));
Jeremy Kerr42dc98c2016-02-25 10:23:14 +080048
Jeremy Kerr7b3bcf72016-02-25 14:29:55 +080049static int send_arp_packet(int fd,
Jeremy Kerr5514f7b2016-02-25 12:41:44 +080050 int ifindex,
Jeremy Kerr760d6ac2016-02-25 14:45:33 +080051 struct arp_packet *eth_arp,
Jeremy Kerr5514f7b2016-02-25 12:41:44 +080052 __be16 ar_op,
53 unsigned char *src_mac,
Jeremy Kerr760d6ac2016-02-25 14:45:33 +080054 struct in_addr *src_ip,
55 unsigned char *dest_mac,
56 struct in_addr *dest_ip)
Jeremy Kerr42dc98c2016-02-25 10:23:14 +080057{
Jeremy Kerr42dc98c2016-02-25 10:23:14 +080058 struct ethhdr *eh = &eth_arp->eh;
59 struct arphdr *arp = &eth_arp->arp;
Jeremy Kerr42dc98c2016-02-25 10:23:14 +080060 struct sockaddr_ll socket_address;
Jeremy Kerrd7865332016-02-25 14:52:50 +080061 int rc;
Jeremy Kerrc10bedb2016-02-25 14:26:38 +080062
63 /* Prepare our link-layer address: raw packet interface,
64 * using the ifindex interface, receiving ARP packets
65 */
Jeremy Kerr5514f7b2016-02-25 12:41:44 +080066 socket_address.sll_family = PF_PACKET;
Jeremy Kerr42dc98c2016-02-25 10:23:14 +080067 socket_address.sll_protocol = htons(ETH_P_ARP);
Jeremy Kerr5514f7b2016-02-25 12:41:44 +080068 socket_address.sll_ifindex = ifindex;
Jeremy Kerr5514f7b2016-02-25 12:41:44 +080069 socket_address.sll_hatype = ARPHRD_ETHER;
Jeremy Kerr5514f7b2016-02-25 12:41:44 +080070 socket_address.sll_pkttype = PACKET_OTHERHOST;
Jeremy Kerr5514f7b2016-02-25 12:41:44 +080071 socket_address.sll_halen = ETH_ALEN;
Jeremy Kerr42dc98c2016-02-25 10:23:14 +080072 memcpy(socket_address.sll_addr, dest_mac, ETH_ALEN);
Jeremy Kerrc10bedb2016-02-25 14:26:38 +080073
74 /* set the frame header */
Jeremy Kerr5514f7b2016-02-25 12:41:44 +080075 memcpy((void *)eh->h_dest, (void *)dest_mac, ETH_ALEN);
76 memcpy((void *)eh->h_source, (void *)src_mac, ETH_ALEN);
Jeremy Kerr42dc98c2016-02-25 10:23:14 +080077 eh->h_proto = htons(ETH_P_ARP);
Jeremy Kerrc10bedb2016-02-25 14:26:38 +080078
79 /* Fill InARP request data for ethernet + ipv4 */
Jeremy Kerr42dc98c2016-02-25 10:23:14 +080080 arp->ar_hrd = htons(ARPHRD_ETHER);
81 arp->ar_pro = htons(ETH_P_ARP);
82 arp->ar_hln = ETH_ALEN;
83 arp->ar_pln = 4;
Jeremy Kerr5514f7b2016-02-25 12:41:44 +080084 arp->ar_op = htons(ar_op);
Jeremy Kerrc10bedb2016-02-25 14:26:38 +080085
86 /* fill arp ethernet mac & ipv4 info */
Jeremy Kerr760d6ac2016-02-25 14:45:33 +080087 memcpy(&eth_arp->src_mac, src_mac, sizeof(eth_arp->src_mac));
88 memcpy(&eth_arp->src_ip, src_ip, sizeof(eth_arp->src_ip));
89 memcpy(&eth_arp->dest_mac, dest_mac, sizeof(eth_arp->dest_mac));
90 memcpy(&eth_arp->dest_ip, dest_ip, sizeof(eth_arp->dest_ip));
Jeremy Kerrc10bedb2016-02-25 14:26:38 +080091
92 /* send the packet */
Jeremy Kerrd7865332016-02-25 14:52:50 +080093 rc = sendto(fd, eth_arp, ETH_ARP_FRAME_LEN, 0,
Jeremy Kerr5514f7b2016-02-25 12:41:44 +080094 (struct sockaddr *)&socket_address,
95 sizeof(socket_address));
Jeremy Kerrd7865332016-02-25 14:52:50 +080096 if (rc < 0)
97 warn("failure sending ARP response");
98
99 return rc;
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800100}
Jeremy Kerr5514f7b2016-02-25 12:41:44 +0800101
Jeremy Kerr7b3bcf72016-02-25 14:29:55 +0800102static void show_mac_addr(const char *name, unsigned char *mac_addr)
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800103{
104 int i;
105 printf("%s MAC address: ", name);
Jeremy Kerr5514f7b2016-02-25 12:41:44 +0800106 for (i = 0; i < 6; i++) {
107 printf("%.2X%c", (unsigned char)mac_addr[i],
108 (i == 5) ? '\n' : ':');
109 }
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800110 return;
111}
Jeremy Kerr5514f7b2016-02-25 12:41:44 +0800112
Jeremy Kerre54e4832016-02-25 17:23:10 +0800113static int get_local_ipaddr(int fd, const char *ifname, struct in_addr *addr)
114{
115 struct sockaddr_in *sa;
116 struct ifreq ifreq;
117 int rc;
118
119 memset(&ifreq, 0, sizeof(ifreq));
120 strcpy(ifreq.ifr_name, ifname);
121
122 rc = ioctl(fd, SIOCGIFADDR, &ifreq);
123 if (rc) {
124 warn("Error querying local address for %s", ifname);
125 return -1;
126 }
127
128 if (ifreq.ifr_addr.sa_family != AF_INET) {
129 warnx("Unknown address family %d in address response",
130 ifreq.ifr_addr.sa_family);
131 return -1;
132 }
133
134 sa = (struct sockaddr_in *)&ifreq.ifr_addr;
135 memcpy(addr, &sa->sin_addr, sizeof(*addr));
136 return 0;
137}
138
Jeremy Kerr115522d2016-02-25 10:24:14 +0800139static void usage(const char *progname)
140{
141 fprintf(stderr, "Usage: %s <interface>\n", progname);
142}
143
144int main(int argc, char **argv)
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800145{
Jeremy Kerr5514f7b2016-02-25 12:41:44 +0800146 /*buffer for ethernet frame */
Jeremy Kerrc10bedb2016-02-25 14:26:38 +0800147 static unsigned char buffer[ETH_FRAME_LEN];
Jeremy Kerr5514f7b2016-02-25 12:41:44 +0800148 static struct ifreq ifreq_buffer;
Jeremy Kerr115522d2016-02-25 10:24:14 +0800149 const char *ifname;
Jeremy Kerr760d6ac2016-02-25 14:45:33 +0800150 int fd, ret;
Jeremy Kerr115522d2016-02-25 10:24:14 +0800151
152 if (argc < 2) {
153 usage(argv[0]);
154 return EXIT_FAILURE;
155 }
156
157 ifname = argv[1];
158
Jeremy Kerrd7865332016-02-25 14:52:50 +0800159 if (strlen(ifname) > IFNAMSIZ)
160 errx(EXIT_FAILURE, "Interface name '%s' is invalid", ifname);
Jeremy Kerr5514f7b2016-02-25 12:41:44 +0800161
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800162 static unsigned char src_mac[6];
Jeremy Kerre54e4832016-02-25 17:23:10 +0800163 static struct in_addr local_ip;
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800164 int ifindex;
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800165
Jeremy Kerrc304ce72016-02-25 18:37:14 +0800166 fd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ARP));
Jeremy Kerrd7865332016-02-25 14:52:50 +0800167 if (fd < 0)
168 err(EXIT_FAILURE, "Error opening ARP socket");
Jeremy Kerrc10bedb2016-02-25 14:26:38 +0800169
170 /* Query local mac address */
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800171 memset(&ifreq_buffer, 0x00, sizeof(ifreq_buffer));
Jeremy Kerr115522d2016-02-25 10:24:14 +0800172 strcpy(ifreq_buffer.ifr_name, ifname);
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800173 ret = ioctl(fd, SIOCGIFHWADDR, &ifreq_buffer);
Jeremy Kerrd7865332016-02-25 14:52:50 +0800174 if (ret < 0)
175 err(EXIT_FAILURE, "Error querying local MAC address");
176
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800177 memcpy(src_mac, ifreq_buffer.ifr_hwaddr.sa_data, ETH_ALEN);
Jeremy Kerr115522d2016-02-25 10:24:14 +0800178 show_mac_addr(ifname, src_mac);
Jeremy Kerrc10bedb2016-02-25 14:26:38 +0800179
180 /* find the ifindex of the interface we're using */
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800181 memset(&ifreq_buffer, 0x00, sizeof(ifreq_buffer));
Jeremy Kerr115522d2016-02-25 10:24:14 +0800182 strcpy(ifreq_buffer.ifr_name, ifname);
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800183 ret = ioctl(fd, SIOCGIFINDEX, &ifreq_buffer);
Jeremy Kerrd7865332016-02-25 14:52:50 +0800184 if (ret < 0)
185 err(EXIT_FAILURE, "Error querying interface %s", ifname);
186
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800187 ifindex = ifreq_buffer.ifr_ifindex;
188
Jeremy Kerrc10bedb2016-02-25 14:26:38 +0800189 /* length of the received frame */
190 int length = 0;
Jeremy Kerr760d6ac2016-02-25 14:45:33 +0800191 static struct arp_packet *inarp_req = (void *)buffer;
192 static struct arp_packet inarp_resp;
Jeremy Kerrc10bedb2016-02-25 14:26:38 +0800193
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800194 while (1) {
Jeremy Kerr5514f7b2016-02-25 12:41:44 +0800195 memset((void *)&inarp_resp, 0, sizeof inarp_resp);
Jeremy Kerr5514f7b2016-02-25 12:41:44 +0800196
Jeremy Kerr77cafea2016-02-25 17:15:28 +0800197 length = recvfrom(fd, buffer, ETH_ARP_FRAME_LEN, 0, NULL, NULL);
198 if (length <= 0) {
199 if (errno == EINTR)
200 continue;
201 err(EXIT_FAILURE, "Error recieving ARP packet");
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800202 }
Jeremy Kerr77cafea2016-02-25 17:15:28 +0800203
204 /* is this an inarp request? */
205 if (ntohs(inarp_req->arp.ar_op) != ARPOP_InREQUEST)
206 continue;
207
208 /* ... for us? */
209 if (memcmp(src_mac, inarp_req->eh.h_dest, ETH_ALEN))
210 continue;
211
212 printf("src mac =%02x:%02x:%02x:%02x:%02x:%02x\n",
213 inarp_req->src_mac[0],
214 inarp_req->src_mac[1],
215 inarp_req->src_mac[2],
216 inarp_req->src_mac[3],
217 inarp_req->src_mac[4],
218 inarp_req->src_mac[5]);
219
220 printf("src ip = %s\n", inet_ntoa(inarp_req->src_ip));
221
Jeremy Kerre54e4832016-02-25 17:23:10 +0800222 ret = get_local_ipaddr(fd, ifname, &local_ip);
223 /* if we don't have a local IP address to send, just drop the
224 * request */
225 if (ret)
226 continue;
227
Jeremy Kerrc304ce72016-02-25 18:37:14 +0800228 ret = send_arp_packet(fd, ifindex, &inarp_resp,
Jeremy Kerr77cafea2016-02-25 17:15:28 +0800229 ARPOP_InREPLY,
230 inarp_req->dest_mac,
Jeremy Kerre54e4832016-02-25 17:23:10 +0800231 &local_ip,
Jeremy Kerr77cafea2016-02-25 17:15:28 +0800232 inarp_req->src_mac,
233 &inarp_req->src_ip);
Jeremy Kerrc304ce72016-02-25 18:37:14 +0800234 if (ret < 0) {
Jeremy Kerr77cafea2016-02-25 17:15:28 +0800235 warn("Error sending response");
236 sleep(1);
237 continue;
238 }
239 memset(buffer, 0, sizeof(buffer));
Jeremy Kerr42dc98c2016-02-25 10:23:14 +0800240 }
241 close(fd);
242 return 0;
243}