1/*
2 * Copyright (c) 2014, University of Washington.
3 * All rights reserved.
4 *
5 * This file is distributed under the terms in the attached LICENSE file.
6 * If you do not find this file, copies can be found by writing to:
7 * ETH Zurich D-INFK, CAB F.78, Universitaetstrasse 6, CH-8092 Zurich.
8 * Attn: Systems Group.
9 */
10
11#include <stdio.h>
12#include <unistd.h>
13#include <stdlib.h>
14#include <string.h>
15#include <netdb.h>
16#include <inttypes.h>
17#include <sys/types.h>
18#include <sys/socket.h>
19#include <netinet/in.h>
20#include <arpa/inet.h>
21#include <sys/time.h>
22#include <strings.h>
23#include <assert.h>
24#include <bitmacros.h>
25#include <pthread.h>
26#include <linux/if_ether.h>
27#include <sys/ioctl.h>
28#define __USE_MISC
29#include <net/if.h>
30#include <netpacket/packet.h>
31#include <net/ethernet.h>
32
33#define IP_PROTO_IPENCAP 4
34#define IP_PROTO_UDP     17
35
36#define IP_HLEN 20
37
38#ifndef BARRELFISH
39struct ip_addr_packed {
40  uint32_t addr;
41} __attribute__ ((packed));
42
43struct ip_addr {
44  uint32_t addr;
45};
46
47typedef struct ip_addr ip_addr_t;
48typedef struct ip_addr_packed ip_addr_p_t;
49#endif
50
51#define TCP_HLEN 20
52#define ip4_addr_get_u32(src_ipaddr) ((src_ipaddr)->addr)
53
54struct ip_hdr {
55  /* version / header length */
56  uint8_t _v_hl;
57  /* type of service */
58  uint8_t _tos;
59  /* total length */
60  uint16_t _len;
61  /* identification */
62  uint16_t _id;
63  /* fragment offset field */
64  uint16_t _offset;
65#define IP_RF 0x8000U        /* reserved fragment flag */
66#define IP_DF 0x4000U        /* dont fragment flag */
67#define IP_MF 0x2000U        /* more fragments flag */
68#define IP_OFFMASK 0x1fffU   /* mask for fragmenting bits */
69  /* time to live */
70  uint8_t _ttl;
71  /* protocol*/
72  uint8_t _proto;
73  /* checksum */
74  uint16_t _chksum;
75  /* source and destination IP addresses */
76  ip_addr_p_t src;
77  ip_addr_p_t dest;
78} __attribute__ ((packed));
79
80#define IPH_V(hdr)  ((hdr)->_v_hl >> 4)
81#define IPH_HL(hdr) ((hdr)->_v_hl & 0x0f)
82#define IPH_TOS(hdr) ((hdr)->_tos)
83#define IPH_LEN(hdr) ((hdr)->_len)
84#define IPH_ID(hdr) ((hdr)->_id)
85#define IPH_OFFSET(hdr) ((hdr)->_offset)
86#define IPH_TTL(hdr) ((hdr)->_ttl)
87#define IPH_PROTO(hdr) ((hdr)->_proto)
88#define IPH_CHKSUM(hdr) ((hdr)->_chksum)
89
90#define IPH_VHL_SET(hdr, v, hl) (hdr)->_v_hl = (((v) << 4) | (hl))
91#define IPH_TOS_SET(hdr, tos) (hdr)->_tos = (tos)
92#define IPH_LEN_SET(hdr, len) (hdr)->_len = (len)
93#define IPH_ID_SET(hdr, id) (hdr)->_id = (id)
94#define IPH_OFFSET_SET(hdr, off) (hdr)->_offset = (off)
95#define IPH_TTL_SET(hdr, ttl) (hdr)->_ttl = (u8_t)(ttl)
96#define IPH_PROTO_SET(hdr, proto) (hdr)->_proto = (u8_t)(proto)
97#define IPH_CHKSUM_SET(hdr, chksum) (hdr)->_chksum = (chksum)
98
99struct udp_hdr {
100  uint16_t src;
101  uint16_t dest;  /* src/dest UDP ports */
102  uint16_t len;
103  uint16_t chksum;
104} __attribute__ ((packed));
105
106#define	timersub(a, b, result)						      \
107  do {									      \
108    (result)->tv_sec = (a)->tv_sec - (b)->tv_sec;			      \
109    (result)->tv_usec = (a)->tv_usec - (b)->tv_usec;			      \
110    if ((result)->tv_usec < 0) {					      \
111      --(result)->tv_sec;						      \
112      (result)->tv_usec += 1000000;					      \
113    }									      \
114  } while (0)
115
116#define BUFSIZE         1024
117#define INBUFSIZE	2048
118#define MAX_ROUNDS      10000000
119
120static struct timeval tvs[MAX_ROUNDS], tst[MAX_ROUNDS];
121
122/*
123 * error - wrapper for perror
124 */
125static void error(char *msg) {
126  perror(msg);
127  exit(1);
128}
129
130static int sockfd; /* socket */
131static struct sockaddr_ll clientaddr; /* client addr */
132static socklen_t clientlen; /* byte size of client's address */
133static int delay = 0;
134static size_t rounds, packets = 0;
135static uint8_t servmac[6] = "\xa0\x36\x9f\x10\x00\xa0";
136
137static void *receiver_func(void *unused)
138{
139  char buf[INBUFSIZE]; /* message buf */
140
141  for(int i = 0; i < rounds; i++) {
142    /*
143     * recvfrom: receive a UDP datagram from a client
144     */
145    struct sockaddr_ll recvaddr;
146    int n;
147
148    for(;;) {
149      int socklen = sizeof(recvaddr);
150      n = recvfrom(sockfd, buf, INBUFSIZE, 0, (struct sockaddr *)&recvaddr, &socklen);
151      if (n < 0)
152	error("ERROR in recvfrom");
153
154      /* printf("socklen = %d, recvaddr = %d, halen = %d\n", socklen, sizeof(recvaddr), recvaddr.sll_halen); */
155      //      assert(socklen == sizeof(recvaddr));
156      if(recvaddr.sll_halen == 6 &&
157	 !memcmp(recvaddr.sll_addr, servmac, 6)) {
158	break;
159      }
160    }
161
162    /* printf("server received %d bytes\n", n); */
163    /* assert(n == BUFSIZE); */
164
165    struct ip_hdr *outer_iphdr = (void *)buf;
166    if(IPH_PROTO(outer_iphdr) == IP_PROTO_IPENCAP) {
167      /* printf("IPIP packet\n"); */
168
169      struct timeval *tstamp = (struct timeval *)&buf[40 + 8 + 8];
170      gettimeofday(&tst[i], NULL);
171      timersub(&tst[i], tstamp, &tvs[i]);
172    } else {
173      assert(IPH_PROTO(outer_iphdr) == IP_PROTO_UDP);
174      /* printf("Decapsulated packet\n"); */
175
176      struct timeval *tstamp = (struct timeval *)&buf[20 + 8 + 8];
177      gettimeofday(&tst[i], NULL);
178      timersub(&tst[i], tstamp, &tvs[i]);
179    }
180
181    /* uint64_t *cnt = (uint64_t *)buf; */
182    packets++;
183    /* if(*cnt != i) { */
184    /*     printf("Packets reordered? %d != %" PRIu64 "\n", i, *cnt); */
185    /*     exit(1); */
186    /* } */
187  }
188
189  return NULL;
190}
191
192static uint16_t
193lwip_standard_chksum(void *dataptr, uint16_t len)
194{
195  uint32_t acc;
196  uint16_t src;
197  uint8_t *octetptr;
198
199  acc = 0;
200  /* dataptr may be at odd or even addresses */
201  octetptr = (uint8_t*)dataptr;
202  while (len > 1) {
203    /* declare first octet as most significant
204       thus assume network order, ignoring host order */
205    src = (*octetptr) << 8;
206    octetptr++;
207    /* declare second octet as least significant */
208    src |= (*octetptr);
209    octetptr++;
210    acc += src;
211    len -= 2;
212  }
213  if (len > 0) {
214    /* accumulate remaining octet */
215    src = (*octetptr) << 8;
216    acc += src;
217  }
218  /* add deferred carry bits */
219  acc = (acc >> 16) + (acc & 0x0000ffffUL);
220  if ((acc & 0xffff0000UL) != 0) {
221    acc = (acc >> 16) + (acc & 0x0000ffffUL);
222  }
223  /* This maybe a little confusing: reorder sum using htons()
224     instead of ntohs() since it has a little less call overhead.
225     The caller must invert bits for Internet sum ! */
226  return htons((uint16_t)acc);
227}
228
229static uint16_t
230inet_chksum(void *dataptr, uint16_t len)
231{
232  return ~lwip_standard_chksum(dataptr, len);
233}
234
235int main(int argc, char **argv) {
236  int portno; /* port to listen on */
237  int optval; /* flag value for setsockopt */
238  char buf[BUFSIZE]; /* message buf */
239
240  /*
241   * check command line arguments
242   */
243  if (argc < 9) {
244    fprintf(stderr, "usage: %s <port> <server IP> <delay us> <rounds> <start_val> <my_ip> <iface_name> <ttl=1_prob>\n", argv[0]);
245    exit(1);
246  }
247  portno = atoi(argv[1]);
248  delay = atoi(argv[3]);
249  rounds = atoi(argv[4]);
250  float ttlprob = atof(argv[8]);
251  assert(rounds < MAX_ROUNDS);
252  size_t start = atoi(argv[5]);
253
254  // Get my source IP address from commandline
255  in_addr_t srcaddr = inet_addr(argv[2]);
256  ip_addr_t srcipaddr = {
257    .addr = srcaddr,
258  };
259  in_addr_t srvaddr = inet_addr(argv[6]);
260
261  /*
262   * socket: create the parent socket
263   */
264  sockfd = socket(AF_PACKET, SOCK_DGRAM, htons(ETH_P_IP));
265  if (sockfd < 0)
266    error("ERROR opening socket");
267
268  struct ifreq ifreq;
269
270  for(int i = 0; i < 100; i++) {
271    memset(&ifreq, 0, sizeof(struct ifreq));
272    ifreq.ifr_ifindex = i;
273    /* struct sockaddr_in sa; */
274    /* memset(&sa, 0, sizeof(struct sockaddr_in)); */
275    /* sa.sin_family = AF_INET; */
276    /* /\* sa.sin_addr.s_addr = 0x80d006ff; *\/ */
277    /* sa.sin_addr.s_addr = 0xff06d080; */
278    /* memcpy(&ifreq.ifr_broadaddr, &sa, sizeof(struct sockaddr_in)); */
279
280    int r = ioctl(sockfd, SIOCGIFNAME, &ifreq);
281    if(r != 0) {
282      continue;
283    }
284    assert(r == 0);
285
286    if(!strcmp(ifreq.ifr_name, argv[7])) {
287      printf("Found index = %d, name = '%s'\n", ifreq.ifr_ifindex, ifreq.ifr_name);
288      break;
289    }
290  }
291
292  static uint8_t clientmac[6] = "\xa0\x36\x9f\x10\x00\xa2";
293  clientlen = sizeof(clientaddr);
294  bzero((char *) &clientaddr, sizeof(clientaddr));
295  clientaddr.sll_family = AF_PACKET;
296  clientaddr.sll_protocol = htons(ETH_P_IP);
297  clientaddr.sll_ifindex = ifreq.ifr_ifindex;
298  clientaddr.sll_halen = 6;
299  memcpy(&clientaddr.sll_addr, clientmac, 6);
300
301  pthread_t sender;
302  int ret = pthread_create(&sender, NULL, receiver_func, NULL);
303  assert(ret == 0);
304
305  struct ip_hdr outer_iphdr = {
306    ._v_hl = 69,
307    ._tos = 0,
308    ._len = htons(IP_HLEN + IP_HLEN + sizeof(struct udp_hdr) + BUFSIZE),
309    ._id = htons(3),
310    ._offset = 0,
311    ._ttl = 6,
312    ._proto = IP_PROTO_IPENCAP,
313    //    ._chksum = htons(0xa1e0),
314    ._chksum = 0,
315    .src.addr = srvaddr,
316    .dest.addr = srcaddr,
317  };
318
319  // Calculate outer_iphdr checksum
320  IPH_CHKSUM_SET(&outer_iphdr, inet_chksum(&outer_iphdr, IP_HLEN));
321
322  struct ip_hdr outer_iphdr2 = {
323    ._v_hl = 69,
324    ._tos = 0,
325    ._len = htons(IP_HLEN + IP_HLEN + sizeof(struct udp_hdr) + BUFSIZE),
326    ._id = htons(3),
327    ._offset = 0,
328    ._ttl = 1,
329    ._proto = IP_PROTO_IPENCAP,
330    /* ._chksum = htons(0xa6e0), */
331    ._chksum = 0,
332    .src.addr = srvaddr,
333    .dest.addr = srcaddr,
334  };
335
336  // Calculate outer_iphdr2 checksum
337  IPH_CHKSUM_SET(&outer_iphdr2, inet_chksum(&outer_iphdr2, IP_HLEN));
338
339  struct ip_hdr inner_iphdr = {
340    ._v_hl = 69,
341    ._tos = 0,
342    ._len = htons(IP_HLEN + sizeof(struct udp_hdr) + BUFSIZE),
343    ._id = htons(3),
344    ._offset = 0,
345    ._ttl = 64,
346    ._proto = IP_PROTO_UDP,
347    //    ._chksum = htons(0x67e7),
348    ._chksum = htons(0),
349    .src.addr = srcaddr,
350    .dest.addr = srvaddr,
351  };
352
353  struct udp_hdr udphdr = {
354    .src = htons(1234),
355    .dest = htons(1234),
356    .len = htons(1024 + sizeof(struct udp_hdr)),
357    .chksum = 0,
358  };
359
360  uint64_t *cnt = (uint64_t *)buf;
361  struct timeval *tstamp = (struct timeval *)&buf[8];
362  memset(tstamp, 0, sizeof(struct timeval));
363
364  for(*cnt = 0; *cnt < rounds; (*cnt)++) {
365      struct timeval oldstamp, diff;
366
367      oldstamp = *tstamp;
368
369      do {
370          gettimeofday(tstamp, NULL);
371          timersub(tstamp, &oldstamp, &diff);
372          /* printf("now = %lu, oldstamp = %lu, diff = %lu, delay = %d\n", */
373          /*        tstamp->tv_sec * 1000000 + tstamp->tv_usec, */
374          /*        oldstamp.tv_sec * 1000000 + oldstamp.tv_usec, */
375          /*        diff.tv_sec * 1000000 + diff.tv_usec, delay); */
376      } while((uint64_t)(diff.tv_sec * 1000000 + diff.tv_usec) < (uint64_t)delay);
377
378      struct iovec out_iovec[4] = {
379	{
380	  .iov_base = &outer_iphdr,
381	  .iov_len = sizeof(struct ip_hdr),
382	},
383	{
384	  .iov_base = &inner_iphdr,
385	  .iov_len = sizeof(struct ip_hdr),
386	},
387	{
388	  .iov_base = &udphdr,
389	  .iov_len = sizeof(udphdr),
390	},
391	{
392	  .iov_base = buf,
393	  .iov_len = BUFSIZE,
394	},
395      };
396      struct msghdr outhdr = {
397	.msg_name = &clientaddr,
398	.msg_namelen = clientlen,
399	.msg_iov = out_iovec,
400	.msg_iovlen = 4,
401	.msg_flags = 0,
402      };
403
404      if((float)rand() / (float)RAND_MAX < ttlprob) {
405	out_iovec[0].iov_base = &outer_iphdr2;
406      }
407
408      int n = sendmsg(sockfd, &outhdr, 0);
409      if (n < 0)
410          error("ERROR in sendto");
411  }
412
413  ret = pthread_cancel(sender);
414  assert(ret == 0);
415
416  unsigned long sum = 0, min = 99999, max = 0;
417
418  for(int i = 0; i < packets; i++) {
419      unsigned long r = tvs[i].tv_sec * 1000000 + tvs[i].tv_usec;
420      unsigned long t = tst[i].tv_sec * 1000000 + tst[i].tv_usec;
421      if(t != 0) {
422          printf("%zd %lu %lu %lu us\n", i + start, t - r, t, r);
423          sum += r;
424          min = MIN(min, r);
425          max = MAX(max, r);
426      }
427  }
428
429  printf("average %lu us, min %lu us, max %lu us\n",
430         sum / packets, min, max);
431
432  return 0;
433}
434