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