extras/libmemif/examples/common/icmp_proto.c

   1 /*
   2  *------------------------------------------------------------------
   3  * Copyright (c) 2017 Cisco and/or its affiliates.
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at:
   7  *
   8  *     http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  *------------------------------------------------------------------
  16  */
  17
  18 #include <stdint.h>
  19 #include <net/if.h>
  20 #include <sys/types.h>
  21 #include <fcntl.h>
  22 #include <sys/ioctl.h>
  23 #include <sys/socket.h>
  24 #include <sys/un.h>
  25 #include <sys/uio.h>
  26 #include <sys/mman.h>
  27 #include <sys/prctl.h>
  28 #include <inttypes.h>
  29 #include <string.h>
  30 #include <stdio.h>
  31 #include <netdb.h>
  32 #include <linux/ip.h>
  33 #include <linux/icmp.h>
  34 #include <arpa/inet.h>
  35 #include <stdlib.h>
  36 #include <netinet/if_ether.h>
  37 #include <net/if_arp.h>
  38 #include <asm/byteorder.h>
  39 #include <byteswap.h>
  40 #include <assert.h>
  41
  42 #include <icmp_proto.h>
  43
  44 static uint16_t
  45 cksum (void *addr, ssize_t len)
  46 {
  47   char *data = (char *) addr;
  48
  49   uint32_t acc = 0xffff;
  50
  51   ssize_t i;
  52   for (i = 0; (i + 1) < len; i += 2)
  53     {
  54       uint16_t word;
  55       memcpy (&word, data + i, 2);
  56       acc += ntohs (word);
  57       if (acc > 0xffff)
  58         acc -= 0xffff;
  59     }
  60
  61   if (len & 1)
  62     {
  63       uint16_t word = 0;
  64       memcpy (&word, data + len - 1, 1);
  65       acc += ntohs (word);
  66       if (acc > 0xffff)
  67         acc -= 0xffff;
  68     }
  69   return htons (~acc);
  70 }
  71
  72 int
  73 print_packet (void *pck)
  74 {
  75   if (pck == NULL)
  76     {
  77       printf ("ICMP_PROTO: no data\n");
  78       return -1;
  79     }
  80   struct iphdr *ip;
  81   struct icmphdr *icmp;
  82   ip = (struct iphdr *) pck;
  83   icmp = (struct icmphdr *) (pck + sizeof (struct iphdr));
  84   printf ("received packet:\n");
  85   printf ("\tiphdr:\n");
  86   printf ("\t\tihl: %u\n\t\tversion: %u\n\t\tlen: %u\n\t\tid: %u\n", ip->ihl,
  87           ip->version, __bswap_16 (ip->tot_len), ip->id);
  88   printf ("\t\tprotocol: %u\n", ip->protocol);
  89
  90   printf ("\t\tsaddr: ");
  91   int i;
  92   for (i = 0; i < 4; i++)
  93     {
  94       printf ("%u.", ((uint8_t *) &ip->saddr)[i]);
  95     }
  96   printf ("\n");
  97
  98   printf ("\t\tdaddr: ");
  99   for (i = 0; i < 4; i++)
 100     {
 101       printf ("%u.", ((uint8_t *) &ip->daddr)[i]);
 102     }
 103   printf ("\n");
 104   printf ("\ticmphdr:\n");
 105   printf ("\t\ttype: %s\n",
 106           (icmp->type == ICMP_ECHO) ? "ICMP_ECHO" : "ICMP_ECHOREPLY");
 107
 108   return 0;
 109 }
 110
 111 static ssize_t
 112 resolve_arp (void *arp)
 113 {
 114   struct arphdr *resp = (struct arphdr *) arp;
 115
 116   resp->ar_hrd = __bswap_16 (ARPHRD_ETHER);
 117
 118   resp->ar_pro = __bswap_16 (0x0800);
 119
 120   resp->ar_hln = 6;
 121   resp->ar_pln = 4;
 122
 123   resp->ar_op = __bswap_16 (ARPOP_REPLY);
 124
 125   return sizeof (struct arphdr);
 126 }
 127
 128 static ssize_t
 129 resolve_eth_arp (struct ether_arp *eth_arp, void *eth_arp_resp,
 130                  uint8_t ip_addr[4])
 131 {
 132   struct ether_arp *resp = (struct ether_arp *) eth_arp_resp;
 133
 134   resolve_arp (&resp->ea_hdr);
 135
 136   memcpy (resp->arp_tha, eth_arp->arp_sha, 6);
 137   memcpy (resp->arp_tpa, eth_arp->arp_spa, 4);
 138
 139   memcpy (
 140     resp->arp_sha,
 141     (((struct ether_header *) (eth_arp_resp - sizeof (struct ether_header)))
 142        ->ether_shost),
 143     6);
 144
 145   memcpy (resp->arp_spa, ip_addr, 4);
 146
 147   return sizeof (struct ether_arp);
 148 }
 149
 150 static ssize_t
 151 resolve_eth (struct ether_header *eth, void *eth_resp, uint8_t hw_addr[6])
 152 {
 153   struct ether_header *resp = (struct ether_header *) eth_resp;
 154   memcpy (resp->ether_dhost, eth->ether_shost, 6);
 155
 156   memcpy (resp->ether_shost, hw_addr, 6);
 157
 158   resp->ether_type = eth->ether_type;
 159
 160   return sizeof (struct ether_header);
 161 }
 162
 163 static ssize_t
 164 resolve_ip (struct iphdr *ip, void *ip_resp, uint8_t ip_addr[4])
 165 {
 166   struct iphdr *resp = (struct iphdr *) ip_resp;
 167   resp->ihl = 5;
 168   resp->version = 4;
 169   resp->tos = 0;
 170   /*len updated later */
 171   resp->tot_len = 0x0000;
 172   resp->id = 0;
 173   resp->frag_off = 0;
 174   resp->ttl = 0x40;
 175   resp->protocol = 1;
 176   ((uint8_t *) &resp->saddr)[0] = ip_addr[0];
 177   ((uint8_t *) &resp->saddr)[1] = ip_addr[1];
 178   ((uint8_t *) &resp->saddr)[2] = ip_addr[2];
 179   ((uint8_t *) &resp->saddr)[3] = ip_addr[3];
 180   resp->daddr = ip->saddr;
 181
 182   /* resp->check =  cksum (resp, sizeof (struct iphdr)); */
 183
 184   return sizeof (struct iphdr);
 185 }
 186
 187 static ssize_t
 188 resolve_icmp (struct icmphdr *icmp, void *icmp_resp)
 189 {
 190   struct icmphdr *resp = (struct icmphdr *) icmp_resp;
 191   resp->type = 0x00;
 192   resp->code = 0;
 193   resp->un.echo.id = icmp->un.echo.id;
 194   resp->un.echo.sequence = icmp->un.echo.sequence;
 195
 196   /*resp->checksum = cksum (resp, sizeof (struct icmphdr)); */
 197
 198   return sizeof (struct icmphdr);
 199 }
 200
 201 static ssize_t
 202 generate_eth (struct ether_header *eh, uint8_t hw_daddr[6])
 203 {
 204   uint8_t hw_addr[6];
 205   int i;
 206   for (i = 0; i < 6; i++)
 207     {
 208       hw_addr[i] = 'a';
 209     }
 210   memcpy (eh->ether_shost, hw_addr, 6);
 211   memcpy (eh->ether_dhost, hw_daddr, 6);
 212
 213   eh->ether_type = 0x0008;
 214
 215   return sizeof (struct ether_header);
 216 }
 217
 218 static ssize_t
 219 generate_ip (struct iphdr *ip, uint8_t saddr[4], uint8_t daddr[4])
 220 {
 221   ip->ihl = 5;
 222   ip->version = 4;
 223   ip->tos = 0;
 224   /*len updated later */
 225   ip->tot_len = 0x5400;
 226   ip->id = 0;
 227   ip->frag_off = 0;
 228   ip->ttl = 0x40;
 229   ip->protocol = 1;
 230   /* saddr */
 231   ((uint8_t *) &ip->saddr)[0] = saddr[0];
 232   ((uint8_t *) &ip->saddr)[1] = saddr[1];
 233   ((uint8_t *) &ip->saddr)[2] = saddr[2];
 234   ((uint8_t *) &ip->saddr)[3] = saddr[3];
 235   /* daddr */
 236   ((uint8_t *) &ip->daddr)[0] = daddr[0];
 237   ((uint8_t *) &ip->daddr)[1] = daddr[1];
 238   ((uint8_t *) &ip->daddr)[2] = daddr[2];
 239   ((uint8_t *) &ip->daddr)[3] = daddr[3];
 240
 241   ip->check = cksum (ip, sizeof (struct iphdr));
 242
 243   return sizeof (struct iphdr);
 244 }
 245
 246 static ssize_t
 247 generate_icmp (struct icmphdr *icmp, uint32_t seq)
 248 {
 249   icmp->type = ICMP_ECHO;
 250   icmp->code = 0;
 251   icmp->un.echo.id = 0;
 252   icmp->un.echo.sequence = seq;
 253
 254   return sizeof (struct icmphdr);
 255 }
 256
 257 int
 258 generate_packet (void *pck, uint32_t *size, uint8_t saddr[4], uint8_t daddr[4],
 259                  uint8_t hw_daddr[6], uint32_t seq)
 260 {
 261   struct ether_header *eh;
 262   struct iphdr *ip;
 263   struct icmphdr *icmp;
 264
 265   *size = 0;
 266
 267   eh = (struct ether_header *) pck;
 268   *size += generate_eth (eh, hw_daddr);
 269
 270   ip = (struct iphdr *) (pck + *size);
 271   *size += generate_ip (ip, saddr, daddr);
 272
 273   icmp = (struct icmphdr *) (pck + *size);
 274   *size += generate_icmp (icmp, seq);
 275
 276   ((struct icmphdr *) (pck + *size - sizeof (struct icmphdr)))->checksum =
 277     cksum (pck + *size - sizeof (struct icmphdr), sizeof (struct icmphdr));
 278
 279   ip->tot_len = __bswap_16 (*size - sizeof (struct ether_header));
 280   ip->check = 0;
 281   ip->check = cksum (ip, sizeof (struct iphdr));
 282
 283   return 0;
 284 }
 285
 286 int
 287 generate_packet2 (void *pck, uint32_t *size, uint8_t saddr[4],
 288                   uint8_t daddr[4], uint8_t hw_daddr[6], uint32_t seq,
 289                   icmpr_flow_mode_t mode)
 290 {
 291   struct ether_header *eh;
 292   struct iphdr *ip;
 293   struct icmphdr *icmp;
 294
 295   *size = 0;
 296
 297   if (mode == ICMPR_FLOW_MODE_ETH)
 298     {
 299       eh = (struct ether_header *) pck;
 300       *size += generate_eth (eh, hw_daddr);
 301     }
 302
 303   ip = (struct iphdr *) (pck + *size);
 304   *size += generate_ip (ip, saddr, daddr);
 305
 306   icmp = (struct icmphdr *) (pck + *size);
 307   *size += generate_icmp (icmp, seq);
 308
 309   ((struct icmphdr *) (pck + *size - sizeof (struct icmphdr)))->checksum =
 310     cksum (pck + *size - sizeof (struct icmphdr), sizeof (struct icmphdr));
 311
 312   ip->tot_len = __bswap_16 (*size - sizeof (struct ether_header));
 313   ip->check = 0;
 314   ip->check = cksum (ip, sizeof (struct iphdr));
 315
 316   return 0;
 317 }
 318
 319 #define GET_HEADER(out, hdr, src, off)                                        \
 320   do                                                                          \
 321     {                                                                         \
 322       out = (hdr *) (src + off);                                              \
 323       off += sizeof (hdr);                                                    \
 324     }                                                                         \
 325   while (0)
 326
 327 int
 328 resolve_packet (void *pck, uint32_t *size, uint8_t ip_addr[4],
 329                 uint8_t hw_addr[6])
 330 {
 331   struct ether_header *eh;
 332   struct ether_arp *eah;
 333   struct iphdr *ip;
 334   struct icmphdr *icmp;
 335   uint32_t offset = 0;
 336
 337   if (pck == NULL)
 338     return 0;
 339
 340 #ifdef ICMP_DBG
 341   print_packet (pck);
 342 #endif
 343
 344   GET_HEADER (eh, struct ether_header, pck, offset);
 345
 346   memcpy (eh->ether_dhost, eh->ether_shost, 6);
 347   memcpy (eh->ether_shost, hw_addr, 6);
 348
 349   if (eh->ether_type == 0x0608)
 350     {
 351       GET_HEADER (eah, struct ether_arp, pck, offset);
 352       struct arphdr *arp = &eah->ea_hdr;
 353
 354       arp->ar_hrd = __bswap_16 (ARPHRD_ETHER);
 355       arp->ar_pro = __bswap_16 (0x0800);
 356
 357       arp->ar_hln = 6;
 358       arp->ar_pln = 4;
 359
 360       arp->ar_op = __bswap_16 (ARPOP_REPLY);
 361
 362       memcpy (eah->arp_tha, eah->arp_sha, 6);
 363       memcpy (eah->arp_tpa, eah->arp_spa, 4);
 364
 365       memcpy (eah->arp_sha, eh->ether_shost, 6);
 366       memcpy (eah->arp_spa, ip_addr, 4);
 367     }
 368
 369   else if (eh->ether_type == 0x0008)
 370     {
 371       GET_HEADER (ip, struct iphdr, pck, offset);
 372
 373       if (ip->protocol == 1)
 374         {
 375           ip->ihl = 5;
 376           ip->version = 4;
 377           ip->tos = 0;
 378           ip->tot_len = 0x0000;
 379           ip->id = 0;
 380           ip->frag_off = 0;
 381           ip->ttl = 0x40;
 382           ip->protocol = 1;
 383           ip->check = 0x0000;
 384           ip->daddr = ip->saddr;
 385           ((uint8_t *) &ip->saddr)[0] = ip_addr[0];
 386           ((uint8_t *) &ip->saddr)[1] = ip_addr[1];
 387           ((uint8_t *) &ip->saddr)[2] = ip_addr[2];
 388           ((uint8_t *) &ip->saddr)[3] = ip_addr[3];
 389
 390           GET_HEADER (icmp, struct icmphdr, pck, offset);
 391
 392           icmp->type = 0x00;
 393           icmp->code = 0;
 394           icmp->checksum = cksum (icmp, sizeof (struct icmphdr));
 395
 396           /* rest is payload */
 397           offset = *size;
 398
 399           ip->tot_len = __bswap_16 (offset - sizeof (struct ether_header));
 400           ip->check = cksum (ip, sizeof (struct iphdr));
 401         }
 402     }
 403
 404   assert (offset == *size && "unsupported protocol");
 405   return 0;
 406 }
 407
 408 int
 409 resolve_packet_with_encap (void **pck_, uint32_t *size, uint8_t ip_addr[4])
 410 {
 411   struct ether_header *eh;
 412   struct iphdr *ip;
 413   struct icmphdr *icmp;
 414   int32_t offset = 0;
 415   uint16_t encap_size = sizeof (struct ether_header);
 416   void *pck = *pck_;
 417
 418   if (pck == NULL)
 419     return 0;
 420
 421   *pck_ -= encap_size;
 422   offset -= encap_size;
 423
 424   GET_HEADER (eh, struct ether_header, pck, offset);
 425
 426   uint8_t hw_daddr[6];
 427   memset (hw_daddr, 0, sizeof (uint8_t) * 6);
 428
 429   generate_eth (eh, hw_daddr);
 430
 431   if (eh->ether_type == 0x0008)
 432     {
 433       GET_HEADER (ip, struct iphdr, pck, offset);
 434
 435       if (ip->protocol == 1)
 436         {
 437           ip->ihl = 5;
 438           ip->version = 4;
 439           ip->tos = 0;
 440           ip->tot_len = 0x0000;
 441           ip->id = 0;
 442           ip->frag_off = 0;
 443           ip->ttl = 0x40;
 444           ip->protocol = 1;
 445           ip->check = 0x0000;
 446           ip->daddr = ip->saddr;
 447           ((uint8_t *) &ip->saddr)[0] = ip_addr[0];
 448           ((uint8_t *) &ip->saddr)[1] = ip_addr[1];
 449           ((uint8_t *) &ip->saddr)[2] = ip_addr[2];
 450           ((uint8_t *) &ip->saddr)[3] = ip_addr[3];
 451
 452           GET_HEADER (icmp, struct icmphdr, pck, offset);
 453
 454           icmp->type = 0x00;
 455           icmp->code = 0;
 456           icmp->checksum = cksum (icmp, sizeof (struct icmphdr));
 457
 458           /* rest is payload */
 459           offset = *size;
 460
 461           ip->tot_len = __bswap_16 (offset - sizeof (struct ether_header));
 462           ip->check = cksum (ip, sizeof (struct iphdr));
 463         }
 464     }
 465
 466   offset += encap_size;
 467
 468   assert (offset != *size &&
 469           "new packet length must be increased by encap size");
 470
 471   /* overwrite packet size */
 472   *size = offset;
 473
 474   return 0;
 475 }