]> git.meshlink.io Git - meshlink/blob - test/pong.c
Make LocalDiscovery work for SPTPS packets.
[meshlink] / test / pong.c
1 /*
2     pong.c -- ICMP echo reply generator
3     Copyright (C) 2013 Guus Sliepen <guus@tinc-vpn.org>
4
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.
9
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.
14
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.
18 */
19
20 #include "../src/system.h"
21
22 uint8_t mymac[6] = {6, 5, 5, 6, 5, 5};
23
24 static ssize_t do_arp(uint8_t *buf, ssize_t len, struct sockaddr_in *in) {
25         struct ether_arp arp;
26         memcpy(&arp, buf + 14, sizeof arp);
27
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)
30                 return 0;
31
32         // Does it match our address?
33         if(memcmp(&in->sin_addr.s_addr, arp.arp_tpa, 4))
34                 return 0;
35
36         // Swap addresses
37         memcpy(buf, buf + 6, 6);
38         memcpy(buf + 6, mymac, 6);
39
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);
45
46         memcpy(buf + 14, &arp, sizeof arp);
47
48         return len;
49 }
50
51 static ssize_t do_ipv4(uint8_t *buf, ssize_t len, struct sockaddr_in *in) {
52         struct ip ip;
53         struct icmp icmp;
54
55         // Does it match our address?
56         if(memcmp(buf, mymac, 6))
57                 return 0;
58
59         memcpy(&ip, buf + 14, sizeof ip);
60         if(memcmp(&ip.ip_dst, &in->sin_addr.s_addr, 4))
61                 return 0;
62
63         // Is it an ICMP echo request?
64         if(ip.ip_p != IPPROTO_ICMP)
65                 return 0;
66
67         memcpy(&icmp, buf + 14 + sizeof ip, sizeof icmp);
68         if(icmp.icmp_type != ICMP_ECHO)
69                 return 0;
70
71         // Return an echo reply
72         memcpy(buf, buf + 6, 6);
73         memcpy(buf + 6, mymac, 6);
74
75         ip.ip_dst = ip.ip_src;
76         memcpy(&ip.ip_src, &in->sin_addr.s_addr, 4);
77
78         icmp.icmp_type = ICMP_ECHOREPLY;
79
80         memcpy(buf + 14, &ip, sizeof ip);
81         memcpy(buf + 14 + sizeof ip, &icmp, sizeof icmp);
82
83         return len;
84 }
85
86 static ssize_t do_ipv6(uint8_t *buf, ssize_t len, struct sockaddr_in6 *in) {
87         return 0;
88 }
89
90 int main(int argc, char *argv[]) {
91         if(argc != 4) {
92                 fprintf(stderr, "Usage: %s <multicast address> <port> <ping address>\n", argv[0]);
93                 return 1;
94         }
95
96         struct addrinfo hints = {}, *ai = NULL;
97         hints.ai_socktype = SOCK_DGRAM;
98         hints.ai_flags = AI_ADDRCONFIG;
99
100         errno = ENOENT;
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));
103                 return 1;
104         }
105
106         int fd;
107         fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol);
108         if(!fd) {
109                 fprintf(stderr, "Could not create socket: %s\n", strerror(errno));
110                 return 1;
111         }
112
113         static const int one = 1;
114         setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, (void *)&one, sizeof one);
115
116         if(bind(fd, ai->ai_addr, ai->ai_addrlen)) {
117                 fprintf(stderr, "Could not bind socket: %s\n", strerror(errno));
118                 return 1;
119         }
120
121         switch(ai->ai_family) {
122                 case AF_INET: {
123                         struct ip_mreq mreq;
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));
130                                 return 1;
131                         }
132 #ifdef IP_MULTICAST_LOOP
133                         setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, (const void *)&one, sizeof one);
134 #endif
135                 } break;
136
137 #ifdef IPV6_JOIN_GROUP
138                 case AF_INET6: {
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));
146                                 return 1;
147                         }
148 #ifdef IPV6_MULTICAST_LOOP
149                         setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, (const void *)&one, sizeof one);
150 #endif
151                 } break;
152 #endif
153
154                 default:
155                         fprintf(stderr, "Multicast for address family %hx unsupported\n", ai->ai_family);
156                         return 1;
157         }
158
159         errno = ENOENT;
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));
163                 return 1;
164         }
165
166         while(true) {
167                 uint8_t buf[10000];
168                 struct sockaddr src;
169                 socklen_t srclen;
170                 ssize_t len = recvfrom(fd, buf, sizeof buf, 0, &src, &srclen);
171                 if(len <= 0)
172                         break;
173
174                 // Ignore short packets.
175                 if(len < 14)
176                         continue;
177
178                 uint16_t type = buf[12] << 8 | buf[13];
179
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);
186                 else
187                         continue;
188
189                 if(len > 0)
190                         sendto(fd, buf, len, 0, ai->ai_addr, ai->ai_addrlen);
191         }
192
193         return 0;
194 }