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 int
 202 resolve_packet (void *in_pck, ssize_t in_size, void *out_pck,
 203                 uint32_t *out_size, uint8_t ip_addr[4], uint8_t hw_addr[6])
 204 {
 205   struct ether_header *eh;
 206   struct ether_arp *eah;
 207   struct iphdr *ip, *ip_out;
 208   struct icmphdr *icmp;
 209   *out_size = 0;
 210
 211   if ((in_pck == NULL) || (out_pck == NULL))
 212     return -1;
 213
 214   eh = (struct ether_header *) in_pck;
 215   *out_size = resolve_eth (eh, out_pck, hw_addr);
 216
 217   if (eh->ether_type == 0x0608)
 218     {
 219       eah = (struct ether_arp *) (in_pck + *out_size);
 220       *out_size += resolve_eth_arp (eah, out_pck + *out_size, ip_addr);
 221     }
 222   else if (eh->ether_type == 0x0008)
 223     {
 224 #ifdef ICMP_DBG
 225       print_packet (in_pck + *out_size);
 226 #endif
 227       ip = (struct iphdr *) (in_pck + *out_size);
 228       ip_out = (struct iphdr *) (out_pck + *out_size);
 229       *out_size += resolve_ip (ip, out_pck + *out_size, ip_addr);
 230       if (ip->protocol == 1)
 231         {
 232           icmp = (struct icmphdr *) (in_pck + *out_size);
 233           *out_size += resolve_icmp (icmp, out_pck + *out_size);
 234           ((struct icmphdr *) (out_pck + *out_size - sizeof (struct icmphdr)))
 235             ->checksum = cksum (out_pck + *out_size - sizeof (struct icmphdr),
 236                                 sizeof (struct icmphdr));
 237           /* payload */
 238           memcpy (out_pck + *out_size, in_pck + *out_size,
 239                   in_size - *out_size);
 240           *out_size = in_size;
 241           ip_out->tot_len =
 242             __bswap_16 (*out_size - sizeof (struct ether_header));
 243           ip_out->check = cksum (ip_out, sizeof (struct iphdr));
 244         }
 245     }
 246   return 0;
 247 }
 248
 249 static ssize_t
 250 generate_eth (struct ether_header *eh, uint8_t hw_daddr[6])
 251 {
 252   uint8_t hw_addr[6];
 253   int i;
 254   for (i = 0; i < 6; i++)
 255     {
 256       hw_addr[i] = 'a';
 257     }
 258   memcpy (eh->ether_shost, hw_addr, 6);
 259   memcpy (eh->ether_dhost, hw_daddr, 6);
 260
 261   eh->ether_type = 0x0008;
 262
 263   return sizeof (struct ether_header);
 264 }
 265
 266 static ssize_t
 267 generate_ip (struct iphdr *ip, uint8_t saddr[4], uint8_t daddr[4])
 268 {
 269   ip->ihl = 5;
 270   ip->version = 4;
 271   ip->tos = 0;
 272   /*len updated later */
 273   ip->tot_len = 0x5400;
 274   ip->id = 0;
 275   ip->frag_off = 0;
 276   ip->ttl = 0x40;
 277   ip->protocol = 1;
 278   /* saddr */
 279   ((uint8_t *) &ip->saddr)[0] = saddr[0];
 280   ((uint8_t *) &ip->saddr)[1] = saddr[1];
 281   ((uint8_t *) &ip->saddr)[2] = saddr[2];
 282   ((uint8_t *) &ip->saddr)[3] = saddr[3];
 283   /* daddr */
 284   ((uint8_t *) &ip->daddr)[0] = daddr[0];
 285   ((uint8_t *) &ip->daddr)[1] = daddr[1];
 286   ((uint8_t *) &ip->daddr)[2] = daddr[2];
 287   ((uint8_t *) &ip->daddr)[3] = daddr[3];
 288
 289   ip->check = cksum (ip, sizeof (struct iphdr));
 290
 291   return sizeof (struct iphdr);
 292 }
 293
 294 static ssize_t
 295 generate_icmp (struct icmphdr *icmp, uint32_t seq)
 296 {
 297   icmp->type = ICMP_ECHO;
 298   icmp->code = 0;
 299   icmp->un.echo.id = 0;
 300   icmp->un.echo.sequence = seq;
 301
 302   return sizeof (struct icmphdr);
 303 }
 304
 305 int
 306 generate_packet (void *pck, uint32_t *size, uint8_t saddr[4], uint8_t daddr[4],
 307                  uint8_t hw_daddr[6], uint32_t seq)
 308 {
 309   struct ether_header *eh;
 310   struct iphdr *ip;
 311   struct icmphdr *icmp;
 312
 313   *size = 0;
 314
 315   eh = (struct ether_header *) pck;
 316   *size += generate_eth (eh, hw_daddr);
 317
 318   ip = (struct iphdr *) (pck + *size);
 319   *size += generate_ip (ip, saddr, daddr);
 320
 321   icmp = (struct icmphdr *) (pck + *size);
 322   *size += generate_icmp (icmp, seq);
 323
 324   ((struct icmphdr *) (pck + *size - sizeof (struct icmphdr)))->checksum =
 325     cksum (pck + *size - sizeof (struct icmphdr), sizeof (struct icmphdr));
 326
 327   ip->tot_len = __bswap_16 (*size - sizeof (struct ether_header));
 328   ip->check = 0;
 329   ip->check = cksum (ip, sizeof (struct iphdr));
 330
 331   return 0;
 332 }
 333
 334 int
 335 generate_packet2 (void *pck, uint32_t *size, uint8_t saddr[4],
 336                   uint8_t daddr[4], uint8_t hw_daddr[6], uint32_t seq,
 337                   icmpr_flow_mode_t mode)
 338 {
 339   struct ether_header *eh;
 340   struct iphdr *ip;
 341   struct icmphdr *icmp;
 342
 343   *size = 0;
 344
 345   if (mode == ICMPR_FLOW_MODE_ETH)
 346     {
 347       eh = (struct ether_header *) pck;
 348       *size += generate_eth (eh, hw_daddr);
 349     }
 350
 351   ip = (struct iphdr *) (pck + *size);
 352   *size += generate_ip (ip, saddr, daddr);
 353
 354   icmp = (struct icmphdr *) (pck + *size);
 355   *size += generate_icmp (icmp, seq);
 356
 357   ((struct icmphdr *) (pck + *size - sizeof (struct icmphdr)))->checksum =
 358     cksum (pck + *size - sizeof (struct icmphdr), sizeof (struct icmphdr));
 359
 360   ip->tot_len = __bswap_16 (*size - sizeof (struct ether_header));
 361   ip->check = 0;
 362   ip->check = cksum (ip, sizeof (struct iphdr));
 363
 364   return 0;
 365 }
 366
 367 #define GET_HEADER(out, hdr, src, off)                                        \
 368   do                                                                          \
 369     {                                                                         \
 370       out = (hdr *) (src + off);                                              \
 371       off += sizeof (hdr);                                                    \
 372     }                                                                         \
 373   while (0)
 374
 375 int
 376 resolve_packet_zero_copy (void *pck, uint32_t *size, uint8_t ip_addr[4],
 377                           uint8_t hw_addr[6])
 378 {
 379   struct ether_header *eh;
 380   struct ether_arp *eah;
 381   struct iphdr *ip;
 382   struct icmphdr *icmp;
 383   uint32_t offset = 0;
 384
 385   if (pck == NULL)
 386     return 0;
 387
 388   GET_HEADER (eh, struct ether_header, pck, offset);
 389
 390   memcpy (eh->ether_dhost, eh->ether_shost, 6);
 391   memcpy (eh->ether_shost, hw_addr, 6);
 392
 393   if (eh->ether_type == 0x0608)
 394     {
 395       GET_HEADER (eah, struct ether_arp, pck, offset);
 396       struct arphdr *arp = &eah->ea_hdr;
 397
 398       arp->ar_hrd = __bswap_16 (ARPHRD_ETHER);
 399       arp->ar_pro = __bswap_16 (0x0800);
 400
 401       arp->ar_hln = 6;
 402       arp->ar_pln = 4;
 403
 404       arp->ar_op = __bswap_16 (ARPOP_REPLY);
 405
 406       memcpy (eah->arp_tha, eah->arp_sha, 6);
 407       memcpy (eah->arp_tpa, eah->arp_spa, 4);
 408
 409       memcpy (eah->arp_sha, eh->ether_shost, 6);
 410       memcpy (eah->arp_spa, ip_addr, 4);
 411     }
 412
 413   else if (eh->ether_type == 0x0008)
 414     {
 415       GET_HEADER (ip, struct iphdr, pck, offset);
 416
 417       if (ip->protocol == 1)
 418         {
 419           ip->ihl = 5;
 420           ip->version = 4;
 421           ip->tos = 0;
 422           ip->tot_len = 0x0000;
 423           ip->id = 0;
 424           ip->frag_off = 0;
 425           ip->ttl = 0x40;
 426           ip->protocol = 1;
 427           ip->check = 0x0000;
 428           ip->daddr = ip->saddr;
 429           ((uint8_t *) &ip->saddr)[0] = ip_addr[0];
 430           ((uint8_t *) &ip->saddr)[1] = ip_addr[1];
 431           ((uint8_t *) &ip->saddr)[2] = ip_addr[2];
 432           ((uint8_t *) &ip->saddr)[3] = ip_addr[3];
 433
 434           GET_HEADER (icmp, struct icmphdr, pck, offset);
 435
 436           icmp->type = 0x00;
 437           icmp->code = 0;
 438           icmp->checksum = cksum (icmp, sizeof (struct icmphdr));
 439
 440           /* rest is payload */
 441           offset = *size;
 442
 443           ip->tot_len = __bswap_16 (offset - sizeof (struct ether_header));
 444           ip->check = cksum (ip, sizeof (struct iphdr));
 445         }
 446     }
 447
 448   assert (offset == *size && "unsupported protocol");
 449   return 0;
 450 }
 451
 452 int
 453 resolve_packet_zero_copy_add_encap (void **pck_, uint32_t *size,
 454                                     uint8_t ip_addr[4])
 455 {
 456   struct ether_header *eh;
 457   struct iphdr *ip;
 458   struct icmphdr *icmp;
 459   int32_t offset = 0;
 460   uint16_t encap_size = sizeof (struct ether_header);
 461   void *pck = *pck_;
 462
 463   if (pck == NULL)
 464     return 0;
 465
 466   *pck_ -= encap_size;
 467   offset -= encap_size;
 468
 469   GET_HEADER (eh, struct ether_header, pck, offset);
 470
 471   uint8_t hw_daddr[6];
 472   memset (hw_daddr, 0, sizeof (uint8_t) * 6);
 473
 474   generate_eth (eh, hw_daddr);
 475
 476   if (eh->ether_type == 0x0008)
 477     {
 478       GET_HEADER (ip, struct iphdr, pck, offset);
 479
 480       if (ip->protocol == 1)
 481         {
 482           ip->ihl = 5;
 483           ip->version = 4;
 484           ip->tos = 0;
 485           ip->tot_len = 0x0000;
 486           ip->id = 0;
 487           ip->frag_off = 0;
 488           ip->ttl = 0x40;
 489           ip->protocol = 1;
 490           ip->check = 0x0000;
 491           ip->daddr = ip->saddr;
 492           ((uint8_t *) &ip->saddr)[0] = ip_addr[0];
 493           ((uint8_t *) &ip->saddr)[1] = ip_addr[1];
 494           ((uint8_t *) &ip->saddr)[2] = ip_addr[2];
 495           ((uint8_t *) &ip->saddr)[3] = ip_addr[3];
 496
 497           GET_HEADER (icmp, struct icmphdr, pck, offset);
 498
 499           icmp->type = 0x00;
 500           icmp->code = 0;
 501           icmp->checksum = cksum (icmp, sizeof (struct icmphdr));
 502
 503           /* rest is payload */
 504           offset = *size;
 505
 506           ip->tot_len = __bswap_16 (offset - sizeof (struct ether_header));
 507           ip->check = cksum (ip, sizeof (struct iphdr));
 508         }
 509     }
 510
 511   offset += encap_size;
 512
 513   assert (offset != *size &&
 514           "new packet length must be increased by encap size");
 515
 516   /* overwrite packet size */
 517   *size = offset;
 518
 519   return 0;
 520 }