icmpecho.cc revision b9400f6c
1#include <algorithm> 2 3#include <stdio.h> 4#include <string.h> 5#include <fcntl.h> 6#include <sys/ioctl.h> 7#include <stdlib.h> 8#include <unistd.h> 9#include <arpa/inet.h> 10#include <net/if.h> 11#include <netinet/ip.h> 12#include <netinet/ip_icmp.h> 13#include <linux/if_tun.h> 14 15#define PACKET_SIZE 1514 16 17int tun_alloc(char *dev) 18{ 19 struct ifreq ifr; 20 int fd, err; 21 22 if ((fd = open("/dev/net/tun", O_RDWR)) < 0) 23 { 24 perror("open"); 25 return -1; 26 } 27 28 memset(&ifr, 0, sizeof(ifr)); 29 ifr.ifr_flags = IFF_TUN | IFF_NO_PI; 30 31 if (*dev) 32 { 33 strncpy(ifr.ifr_name, dev, IFNAMSIZ); 34 } 35 36 if ((err = ioctl(fd, TUNSETIFF, (void *) &ifr)) < 0) 37 { 38 perror("ioctl"); 39 close(fd); 40 return err; 41 } 42 strcpy(dev, ifr.ifr_name); 43 44 return fd; 45} 46 47void icmp_echo(int fd, const void* input, const void* payload, int len) 48{ 49 const struct iphdr* iphdr = static_cast<const struct iphdr*>(input); 50 const struct icmphdr* icmphdr = static_cast<const struct icmphdr*>(payload); 51 const int icmphdr_size = sizeof(*icmphdr); 52 53 if (icmphdr->type == ICMP_ECHO) 54 { 55 char source[INET_ADDRSTRLEN]; 56 char dest[INET_ADDRSTRLEN]; 57 inet_ntop(AF_INET, &iphdr->saddr, source, INET_ADDRSTRLEN); 58 inet_ntop(AF_INET, &iphdr->daddr, dest, INET_ADDRSTRLEN); 59 printf("%s > %s: ", source, dest); 60 printf("ICMP echo request, id %d, seq %d, length %d\n", 61 ntohs(icmphdr->un.echo.id), 62 ntohs(icmphdr->un.echo.sequence), 63 len - iphdr->ihl*4); 64 } 65 66 union 67 { 68 unsigned char output[PACKET_SIZE]; 69 struct 70 { 71 struct iphdr iphdr; 72 struct icmphdr icmphdr; 73 } out; 74 }; 75 76 memcpy(output, input, len); 77 out.icmphdr.type = ICMP_ECHOREPLY; 78 out.icmphdr.checksum += ICMP_ECHO; // FIXME: not portable 79 std::swap(out.iphdr.saddr, out.iphdr.daddr); 80 write(fd, output, len); 81} 82 83uint16_t iphdr_checksum(const void* buf, int len) 84{ 85 const uint16_t* data = static_cast<const uint16_t*>(buf); 86 int sum = 0; 87 for (int i = 0; i < len; i+=2) 88 { 89 sum += *data++; 90 } 91 // while (sum >> 16) 92 sum = (sum & 0xFFFF) + (sum >> 16); 93 return ~sum; 94} 95 96int main() 97{ 98 union 99 { 100 unsigned char buf[PACKET_SIZE]; 101 struct iphdr iphdr; 102 }; 103 104 const int iphdr_size = sizeof iphdr; 105 106 char ifname[IFNAMSIZ] = "tun%d"; 107 int fd = tun_alloc(ifname); 108 109 if (fd < 0) 110 { 111 fprintf(stderr, "tunnel interface allocation failed\n"); 112 exit(1); 113 } 114 115 printf("allocted tunnel interface %s\n", ifname); 116 sleep(1); 117 118 for (;;) 119 { 120 int nread = read(fd, buf, sizeof(buf)); 121 if (nread < 0) 122 { 123 perror("read"); 124 close(fd); 125 exit(1); 126 } 127 printf("read %d bytes from tunnel interface %s.\n", nread, ifname); 128 129 if (nread >= iphdr_size 130 && iphdr.version == 4 131 && iphdr.ihl*4 >= iphdr_size 132 && iphdr.ihl*4 <= nread 133 && iphdr.tot_len == htons(nread) 134 && iphdr_checksum(buf, iphdr.ihl*4) == 0) 135 { 136 const void* payload = buf + iphdr.ihl*4; 137 if (iphdr.protocol == 1) 138 { 139 icmp_echo(fd, buf, payload, nread); 140 } 141 } 142 else 143 { 144 printf("bad packet\n"); 145 for (int i = 0; i < nread; ++i) 146 { 147 if (i % 4 == 0) printf("\n"); 148 printf("%02x ", buf[i]); 149 } 150 printf("\n"); 151 } 152 } 153 154 return 0; 155} 156