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