2 pong.c -- ICMP echo reply generator
3 Copyright (C) 2013 Guus Sliepen <guus@tinc-vpn.org>
5 This program is free software; you can redistribute it and/or modify
6 it under the terms of the GNU General Public License as published by
7 the Free Software Foundation; either version 2 of the License, or
8 (at your option) any later version.
10 This program is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU General Public License for more details.
15 You should have received a copy of the GNU General Public License along
16 with this program; if not, write to the Free Software Foundation, Inc.,
17 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
20 #include "../src/system.h"
22 uint8_t mymac
[6] = {6, 5, 5, 6, 5, 5};
24 static ssize_t
do_arp(uint8_t *buf
, ssize_t len
, struct sockaddr_in
*in
) {
26 memcpy(&arp
, buf
+ 14, sizeof arp
);
28 // Is it a valid ARP request?
29 if(ntohs(arp
.arp_hrd
) != ARPHRD_ETHER
|| ntohs(arp
.arp_pro
) != ETH_P_IP
|| arp
.arp_hln
!= ETH_ALEN
|| arp
.arp_pln
!= sizeof in
->sin_addr
.s_addr
|| ntohs(arp
.arp_op
) != ARPOP_REQUEST
)
32 // Does it match our address?
33 if(memcmp(&in
->sin_addr
.s_addr
, arp
.arp_tpa
, 4))
37 memcpy(buf
, buf
+ 6, 6);
38 memcpy(buf
+ 6, mymac
, 6);
40 arp
.arp_op
= htons(ARPOP_REPLY
);
41 memcpy(arp
.arp_tpa
, arp
.arp_spa
, sizeof arp
.arp_tpa
);
42 memcpy(arp
.arp_tha
, arp
.arp_sha
, sizeof arp
.arp_tha
);
43 memcpy(arp
.arp_spa
, &in
->sin_addr
.s_addr
, sizeof in
->sin_addr
.s_addr
);
44 memcpy(arp
.arp_sha
, mymac
, 6);
46 memcpy(buf
+ 14, &arp
, sizeof arp
);
51 static ssize_t
do_ipv4(uint8_t *buf
, ssize_t len
, struct sockaddr_in
*in
) {
55 // Does it match our address?
56 if(memcmp(buf
, mymac
, 6))
59 memcpy(&ip
, buf
+ 14, sizeof ip
);
60 if(memcmp(&ip
.ip_dst
, &in
->sin_addr
.s_addr
, 4))
63 // Is it an ICMP echo request?
64 if(ip
.ip_p
!= IPPROTO_ICMP
)
67 memcpy(&icmp
, buf
+ 14 + sizeof ip
, sizeof icmp
);
68 if(icmp
.icmp_type
!= ICMP_ECHO
)
71 // Return an echo reply
72 memcpy(buf
, buf
+ 6, 6);
73 memcpy(buf
+ 6, mymac
, 6);
75 ip
.ip_dst
= ip
.ip_src
;
76 memcpy(&ip
.ip_src
, &in
->sin_addr
.s_addr
, 4);
78 icmp
.icmp_type
= ICMP_ECHOREPLY
;
80 memcpy(buf
+ 14, &ip
, sizeof ip
);
81 memcpy(buf
+ 14 + sizeof ip
, &icmp
, sizeof icmp
);
86 static ssize_t
do_ipv6(uint8_t *buf
, ssize_t len
, struct sockaddr_in6
*in
) {
90 int main(int argc
, char *argv
[]) {
92 fprintf(stderr
, "Usage: %s <multicast address> <port> <ping address>\n", argv
[0]);
96 struct addrinfo hints
= {}, *ai
= NULL
;
97 hints
.ai_socktype
= SOCK_DGRAM
;
98 hints
.ai_flags
= AI_ADDRCONFIG
;
101 if(getaddrinfo(argv
[1], argv
[2], &hints
, &ai
) || !ai
) {
102 fprintf(stderr
, "Could not resolve %s port %s: %s\n", argv
[1], argv
[2], strerror(errno
));
107 fd
= socket(ai
->ai_family
, ai
->ai_socktype
, ai
->ai_protocol
);
109 fprintf(stderr
, "Could not create socket: %s\n", strerror(errno
));
113 static const int one
= 1;
114 setsockopt(fd
, SOL_SOCKET
, SO_REUSEADDR
, (void *)&one
, sizeof one
);
116 if(bind(fd
, ai
->ai_addr
, ai
->ai_addrlen
)) {
117 fprintf(stderr
, "Could not bind socket: %s\n", strerror(errno
));
121 switch(ai
->ai_family
) {
124 struct sockaddr_in in
;
125 memcpy(&in
, ai
->ai_addr
, sizeof in
);
126 mreq
.imr_multiaddr
.s_addr
= in
.sin_addr
.s_addr
;
127 mreq
.imr_interface
.s_addr
= htonl(INADDR_ANY
);
128 if(setsockopt(fd
, IPPROTO_IP
, IP_ADD_MEMBERSHIP
, (void *)&mreq
, sizeof mreq
)) {
129 fprintf(stderr
, "Cannot join multicast group: %s\n", strerror(errno
));
132 #ifdef IP_MULTICAST_LOOP
133 setsockopt(fd
, IPPROTO_IP
, IP_MULTICAST_LOOP
, (const void *)&one
, sizeof one
);
137 #ifdef IPV6_JOIN_GROUP
139 struct ipv6_mreq mreq
;
140 struct sockaddr_in6 in6
;
141 memcpy(&in6
, ai
->ai_addr
, sizeof in6
);
142 memcpy(&mreq
.ipv6mr_multiaddr
, &in6
.sin6_addr
, sizeof mreq
.ipv6mr_multiaddr
);
143 mreq
.ipv6mr_interface
= in6
.sin6_scope_id
;
144 if(setsockopt(fd
, IPPROTO_IPV6
, IPV6_JOIN_GROUP
, (void *)&mreq
, sizeof mreq
)) {
145 fprintf(stderr
, "Cannot join multicast group: %s\n", strerror(errno
));
148 #ifdef IPV6_MULTICAST_LOOP
149 setsockopt(fd
, IPPROTO_IPV6
, IPV6_MULTICAST_LOOP
, (const void *)&one
, sizeof one
);
155 fprintf(stderr
, "Multicast for address family %hx unsupported\n", ai
->ai_family
);
160 struct addrinfo
*ai2
= NULL
;
161 if(getaddrinfo(argv
[3], NULL
, &hints
, &ai2
) || !ai2
) {
162 fprintf(stderr
, "Could not resolve %s: %s\n", argv
[3], strerror(errno
));
170 ssize_t len
= recvfrom(fd
, buf
, sizeof buf
, 0, &src
, &srclen
);
174 // Ignore short packets.
178 uint16_t type
= buf
[12] << 8 | buf
[13];
180 if(ai2
->ai_family
== AF_INET
&& type
== ETH_P_IP
)
181 len
= do_ipv4(buf
, len
, (struct sockaddr_in
*)ai2
->ai_addr
);
182 else if(ai2
->ai_family
== AF_INET
&& type
== ETH_P_ARP
)
183 len
= do_arp(buf
, len
, (struct sockaddr_in
*)ai2
->ai_addr
);
184 else if(ai2
->ai_family
== AF_INET6
&& type
== ETH_P_IPV6
)
185 len
= do_ipv6(buf
, len
, (struct sockaddr_in6
*)ai2
->ai_addr
);
190 sendto(fd
, buf
, len
, 0, ai
->ai_addr
, ai
->ai_addrlen
);