test/pong.c

   1 /*
   2     pong.c -- ICMP echo reply generator
   3     Copyright (C) 2013-2017 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 #include "../src/ethernet.h"
  23
  24 uint8_t mymac[6] = {6, 5, 5, 6, 5, 5};
  25
  26 static ssize_t do_arp(uint8_t *buf, ssize_t len, struct sockaddr_in *in) {
  27         struct ether_arp arp;
  28         memcpy(&arp, buf + 14, sizeof(arp));
  29
  30         // Is it a valid ARP request?
  31         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                 return 0;
  33         }
  34
  35         // Does it match our address?
  36         if(memcmp(&in->sin_addr.s_addr, arp.arp_tpa, 4)) {
  37                 return 0;
  38         }
  39
  40         // Swap addresses
  41         memcpy(buf, buf + 6, 6);
  42         memcpy(buf + 6, mymac, 6);
  43
  44         arp.arp_op = htons(ARPOP_REPLY);
  45         memcpy(arp.arp_tpa, arp.arp_spa, sizeof(arp.arp_tpa));
  46         memcpy(arp.arp_tha, arp.arp_sha, sizeof(arp.arp_tha));
  47         memcpy(arp.arp_spa, &in->sin_addr.s_addr, sizeof(in->sin_addr.s_addr));
  48         memcpy(arp.arp_sha, mymac, 6);
  49
  50         memcpy(buf + 14, &arp, sizeof(arp));
  51
  52         return len;
  53 }
  54
  55 static ssize_t do_ipv4(uint8_t *buf, ssize_t len, struct sockaddr_in *in) {
  56         struct ip ip;
  57         struct icmp icmp;
  58
  59         // Does it match our address?
  60         if(memcmp(buf, mymac, 6)) {
  61                 return 0;
  62         }
  63
  64         memcpy(&ip, buf + 14, sizeof(ip));
  65
  66         if(memcmp(&ip.ip_dst, &in->sin_addr.s_addr, 4)) {
  67                 return 0;
  68         }
  69
  70         // Is it an ICMP echo request?
  71         if(ip.ip_p != IPPROTO_ICMP) {
  72                 return 0;
  73         }
  74
  75         memcpy(&icmp, buf + 14 + sizeof(ip), sizeof(icmp));
  76
  77         if(icmp.icmp_type != ICMP_ECHO) {
  78                 return 0;
  79         }
  80
  81         // Return an echo reply
  82         memcpy(buf, buf + 6, 6);
  83         memcpy(buf + 6, mymac, 6);
  84
  85         ip.ip_dst = ip.ip_src;
  86         memcpy(&ip.ip_src, &in->sin_addr.s_addr, 4);
  87
  88         icmp.icmp_type = ICMP_ECHOREPLY;
  89
  90         memcpy(buf + 14, &ip, sizeof(ip));
  91         memcpy(buf + 14 + sizeof(ip), &icmp, sizeof(icmp));
  92
  93         return len;
  94 }
  95
  96 static ssize_t do_ipv6(uint8_t *buf, ssize_t len, struct sockaddr_in6 *in) {
  97         return 0;
  98 }
  99
 100 int main(int argc, char *argv[]) {
 101         if(argc != 4) {
 102                 fprintf(stderr, "Usage: %s <multicast address> <port> <ping address>\n", argv[0]);
 103                 return 1;
 104         }
 105
 106         struct addrinfo hints = {}, *ai = NULL;
 107         hints.ai_socktype = SOCK_DGRAM;
 108         hints.ai_flags = AI_ADDRCONFIG;
 109
 110         errno = ENOENT;
 111
 112         if(getaddrinfo(argv[1], argv[2], &hints, &ai) || !ai) {
 113                 fprintf(stderr, "Could not resolve %s port %s: %s\n", argv[1], argv[2], strerror(errno));
 114                 return 1;
 115         }
 116
 117         int fd;
 118         fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol);
 119
 120         if(!fd) {
 121                 fprintf(stderr, "Could not create socket: %s\n", strerror(errno));
 122                 return 1;
 123         }
 124
 125         static const int one = 1;
 126         setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, (void *)&one, sizeof(one));
 127
 128         if(bind(fd, ai->ai_addr, ai->ai_addrlen)) {
 129                 fprintf(stderr, "Could not bind socket: %s\n", strerror(errno));
 130                 return 1;
 131         }
 132
 133         switch(ai->ai_family) {
 134         case AF_INET: {
 135                 struct ip_mreq mreq;
 136                 struct sockaddr_in in;
 137                 memcpy(&in, ai->ai_addr, sizeof(in));
 138                 mreq.imr_multiaddr.s_addr = in.sin_addr.s_addr;
 139                 mreq.imr_interface.s_addr = htonl(INADDR_ANY);
 140
 141                 if(setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, (void *)&mreq, sizeof(mreq))) {
 142                         fprintf(stderr, "Cannot join multicast group: %s\n", strerror(errno));
 143                         return 1;
 144                 }
 145
 146 #ifdef IP_MULTICAST_LOOP
 147                 setsockopt(fd, IPPROTO_IP, IP_MULTICAST_LOOP, (const void *)&one, sizeof(one));
 148 #endif
 149         }
 150         break;
 151
 152 #ifdef IPV6_JOIN_GROUP
 153
 154         case AF_INET6: {
 155                 struct ipv6_mreq mreq;
 156                 struct sockaddr_in6 in6;
 157                 memcpy(&in6, ai->ai_addr, sizeof(in6));
 158                 memcpy(&mreq.ipv6mr_multiaddr, &in6.sin6_addr, sizeof(mreq.ipv6mr_multiaddr));
 159                 mreq.ipv6mr_interface = in6.sin6_scope_id;
 160
 161                 if(setsockopt(fd, IPPROTO_IPV6, IPV6_JOIN_GROUP, (void *)&mreq, sizeof(mreq))) {
 162                         fprintf(stderr, "Cannot join multicast group: %s\n", strerror(errno));
 163                         return 1;
 164                 }
 165
 166 #ifdef IPV6_MULTICAST_LOOP
 167                 setsockopt(fd, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, (const void *)&one, sizeof(one));
 168 #endif
 169         }
 170         break;
 171 #endif
 172
 173         default:
 174                 fprintf(stderr, "Multicast for address family %x unsupported\n", ai->ai_family);
 175                 return 1;
 176         }
 177
 178         errno = ENOENT;
 179         struct addrinfo *ai2 = NULL;
 180
 181         if(getaddrinfo(argv[3], NULL, &hints, &ai2) || !ai2) {
 182                 fprintf(stderr, "Could not resolve %s: %s\n", argv[3], strerror(errno));
 183                 return 1;
 184         }
 185
 186         while(true) {
 187                 uint8_t buf[10000];
 188                 struct sockaddr src;
 189                 socklen_t srclen;
 190                 ssize_t len = recvfrom(fd, buf, sizeof(buf), 0, &src, &srclen);
 191
 192                 if(len <= 0) {
 193                         break;
 194                 }
 195
 196                 // Ignore short packets.
 197                 if(len < 14) {
 198                         continue;
 199                 }
 200
 201                 uint16_t type = buf[12] << 8 | buf[13];
 202
 203                 if(ai2->ai_family == AF_INET && type == ETH_P_IP) {
 204                         len = do_ipv4(buf, len, (struct sockaddr_in *)ai2->ai_addr);
 205                 } else if(ai2->ai_family == AF_INET && type == ETH_P_ARP) {
 206                         len = do_arp(buf, len, (struct sockaddr_in *)ai2->ai_addr);
 207                 } else if(ai2->ai_family == AF_INET6 && type == ETH_P_IPV6) {
 208                         len = do_ipv6(buf, len, (struct sockaddr_in6 *)ai2->ai_addr);
 209                 } else {
 210                         continue;
 211                 }
 212
 213                 if(len > 0) {
 214                         sendto(fd, buf, len, 0, ai->ai_addr, ai->ai_addrlen);
 215                 }
 216         }
 217
 218         return 0;
 219 }