misc/network_servers/vtl/vtl_util.cc

   1 #include "vtl_util.h"
   2 #include <assert.h>
   3
   4
   5 void dbg_print_pkt_info(RawEthernetPacket * pkt) {
   6   unsigned short src_port;
   7   unsigned short dest_port;
   8   string dest_str;
   9   string src_str;
  10
  11   unsigned long seq_num = GET_TCP_SEQ_NUM(pkt->data);
  12   unsigned long ack_num = GET_TCP_ACK_NUM(pkt->data);
  13
  14   src_port = GET_TCP_SRC_PORT(pkt->data);
  15   dest_port = GET_TCP_DST_PORT(pkt->data);
  16
  17   dest_str = ip_to_string(GET_IP_DST(pkt->data));
  18   src_str = ip_to_string(GET_IP_SRC(pkt->data));
  19
  20   JRLDBG("Packet: %s:%d-%s:%d seq: %lu, ack: %lu\n", src_str.c_str(), ntohs(src_port), dest_str.c_str(), ntohs(dest_port),
  21          seq_num, ack_num);
  22
  23   return;
  24
  25 }
  26
  27 void dbg_print_pkt(RawEthernetPacket * pkt) {
  28   unsigned int x;
  29   int i;
  30   char pkt_line[128];
  31   unsigned int pkt_size = pkt->get_size() - 1;
  32
  33   JRLDBG("Packet Dump: (pkt_size=%lu) \n", pkt->get_size());
  34
  35   for (x = 0; x < pkt_size;) {
  36     sprintf(pkt_line, "\t%.4x:  ", x);
  37
  38     for (i = 0; i < 16; i += 2) {
  39       if (pkt_size < x + i) {
  40         break;
  41       }
  42
  43       if (pkt_size == x + i) {
  44         sprintf(pkt_line, "%s%.2x ", pkt_line, *(unsigned char *)(pkt->data + i + x));
  45       } else {
  46
  47         sprintf(pkt_line, "%s%.4x  ", pkt_line, ntohs(*(unsigned short *)(pkt->data + i + x)));
  48       }
  49     }
  50
  51     JRLDBG("%s\n", pkt_line);
  52
  53     x += 16;
  54   }
  55 }
  56
  57 void dbg_print_buf(unsigned char * buf, unsigned int len) {
  58   unsigned int x;
  59   int i;
  60   char pkt_line[128];
  61
  62   JRLDBG("Buf Dump: (len=%d) \n", len);
  63
  64   for (x = 0; x < len-1;) {
  65     sprintf(pkt_line, "\t%.4x:  ", x);
  66
  67     for (i = 0; i < 16; i += 2) {
  68       if ((len - 1) < x + i) {
  69         break;
  70       }
  71
  72       if (len == x + i + 1) {
  73         sprintf(pkt_line, "%s%.2x ", pkt_line, *(unsigned char *)(buf + i + x));
  74       } else {
  75
  76         sprintf(pkt_line, "%s%.4x  ", pkt_line, ntohs(*(unsigned short *)(buf + i + x)));
  77       }
  78     }
  79
  80     JRLDBG("%s\n", pkt_line);
  81
  82     x += 16;
  83   }
  84
  85 }
  86 /*
  87   void do_binary_to_ipaddress(unsigned char* ip,IPADDRESS& ipaddress)
  88   {
  89   ipaddress.a1=ip[0];
  90   ipaddress.a2=ip[1];
  91   ipaddress.a3=ip[2];
  92   ipaddress.a4=ip[3];
  93   }
  94
  95   void do_ipaddress_to_string(IPADDRESS ipaddress,char* buffer)
  96   {
  97   sprintf(buffer,"%d.%d.%d.%d",ipaddress.a1,ipaddress.a2,ipaddress.a3,ipaddress.a4);
  98   }
  99
 100   void do_binary_to_string(unsigned char* ip,char* buffer)
 101   {
 102   IPADDRESS ipaddress;
 103   do_binary_to_ipaddress(ip,ipaddress);
 104   do_ipaddress_to_string(ipaddress,buffer);
 105   }
 106 */
 107
 108 int get_mss(RawEthernetPacket * pkt) {
 109   unsigned long ip_hdr_len = GET_IP_HDR_LEN(pkt->data);
 110   unsigned short tcp_hdr_len = (*(pkt->data + ETH_HDR_LEN + ip_hdr_len + 12) & 0xf0) >> 2;
 111   int offset = 0;
 112   int len = tcp_hdr_len - 20;
 113   unsigned short mss;
 114
 115   char * opts = (pkt->data + ETH_HDR_LEN + ip_hdr_len + 20);
 116
 117   if (len <= 0) {
 118     return -1;
 119   }
 120
 121   while (offset < len) {
 122     if (*(opts + offset) == 0x00) {
 123       break;
 124     } else if (*(opts + offset) == 0x01) {
 125       offset++;
 126     } else if (*(opts + offset) == 0x02) {
 127       mss = (*(unsigned short *)(opts + offset + 2));
 128       offset += *(opts + offset + 1);
 129       return (int)ntohs(mss);
 130     } else {
 131       offset += *(opts + offset + 1);
 132     }
 133   }
 134   return -1;
 135 }
 136
 137 int parse_tcp_options(tcp_opts_t * options, RawEthernetPacket * pkt) {
 138   ASSERT((options != NULL) && (pkt != NULL));
 139
 140   memset(options, 0, sizeof(options));
 141
 142   int offset = 0;
 143   char * opts = TCP_OPTS(pkt->data);
 144   int opt_len = GET_TCP_OPTS_LEN(pkt->data);
 145   int field_len = 0;
 146   if (opt_len == 0) {
 147     // no options
 148     return -1;
 149   }
 150
 151
 152   while (offset < opt_len) {
 153     if (*(opts + offset) == 0x00) {
 154       break;
 155     } else if (*(opts + offset) == 0x01) {
 156       offset++;
 157     } else if (*(opts + offset) == 0x02) {
 158       options->mss = ntohs(*(unsigned short *)(opts + offset + 2));
 159       offset += *(opts + offset + 1);
 160     } else if (*(opts + offset) == 0x03) {
 161       options->window = *(unsigned char *)(opts + offset + 2);
 162       offset += *(opts + offset + 1);
 163     } else if (*(opts + offset) == 0x04) {
 164       // SACK OK
 165       options->sack_ok = 1;
 166       offset += 2;
 167     } else if (*(opts + offset) == 0x05) {
 168       field_len = *(opts + offset + 1);
 169       options->sack_entries = (unsigned long *)malloc(field_len - 2);
 170       memcpy(options->sack_entries, opts + offset + 2, field_len - 2);
 171       offset += field_len;
 172     } else if (*(opts + offset) == 0x08) {
 173       offset += 2;
 174       options->local_ts = *(unsigned long *)(opts + offset);
 175       offset += 4;
 176       options->remote_ts = *(unsigned long *)(opts + offset);
 177       offset += 4;
 178     } else {
 179       // default handler to skip what we don't look for
 180       offset += *(opts + offset + 1);
 181     }
 182   }
 183   return 0;
 184 }
 185
 186 int set_tcp_options(tcp_opts_t * options, unsigned long opt_flags, RawEthernetPacket * pkt) {
 187   char * pkt_opts = TCP_DATA(pkt->data);
 188   int offset = 0;
 189
 190
 191   if (opt_flags & TCP_OPTS_MSS) {
 192     *(pkt_opts + offset) = 0x02;
 193     *(pkt_opts + offset + 1) = 0x04;
 194     *(unsigned short *)(pkt_opts + offset + 2) = ntohs(options->mss);
 195     offset += 4;
 196   }
 197
 198
 199   *(pkt_opts + offset) = 0x00;
 200   offset++;
 201
 202   SET_IP_TOTAL_LEN(pkt->data, GET_IP_TOTAL_LEN(pkt->data) + offset);
 203   compute_ip_checksum(pkt);
 204   SET_TCP_HDR_LEN(pkt->data, GET_TCP_HDR_LEN(pkt->data) + offset);
 205   compute_tcp_checksum(pkt);
 206   pkt->set_size(pkt->get_size() + offset);
 207
 208   return 0;
 209 }
 210
 211 unsigned long get_tcp_timestamp(char *opts, int len) {
 212   int offset = 0;
 213   unsigned long timestamp = 0;
 214   unsigned long * ts_ptr;
 215
 216   while (offset < len) {
 217     if (*(opts + offset) == 0x00) {
 218       break;
 219     } else if (*(opts + offset) == 0x01) {
 220       offset++;
 221     } else if (*(opts + offset) == 0x08) {
 222       offset += 2;
 223       ts_ptr = (unsigned long *)(opts + offset);
 224       timestamp = (*ts_ptr);
 225       break;
 226     } else if (*(opts + offset) == 0x02) {
 227       offset += *(opts + offset + 1);
 228     } else if (*(opts + offset) == 0x03) {
 229       offset += *(opts + offset + 1);
 230     } else if (*(opts + offset) == 0x04) {
 231       // SACK OK
 232       offset += 2;
 233     } else if (*(opts + offset) == 0x05) {
 234       offset += *(opts + offset + 1);
 235     } else {
 236       offset += *(opts + offset + 1);
 237       //JRLDBG("Could not find timestamp\n");
 238       //break;
 239     }
 240   }
 241   return timestamp;
 242 }
 243
 244 void set_tcp_timestamp(char * ts_opt, unsigned long local_ts, unsigned long remote_ts) {
 245   int offset = 0;
 246
 247   //  *(ts_opt + offset) = 0x01;
 248   //offset++;
 249   //*(ts_opt + offset) = 0x01;
 250   //offset++;
 251   *(ts_opt + offset) = 0x08;
 252   offset++;
 253   *(ts_opt + offset) = 0x0a;
 254   offset++;
 255
 256   *(unsigned long *)(ts_opt + offset) = local_ts;
 257   offset += sizeof(unsigned long);
 258
 259   *(unsigned long *)(ts_opt + offset) = remote_ts;
 260
 261   return;
 262 }
 263
 264
 265
 266 int pkt_has_timestamp(RawEthernetPacket * pkt) {
 267   unsigned short ip_hdr_len = GET_IP_HDR_LEN(pkt->data);
 268   unsigned short tcp_hdr_len = (*(pkt->data + ETH_HDR_LEN + ip_hdr_len + 12) & 0xf0) >> 2;
 269   int offset = 0;
 270   int len = tcp_hdr_len - 20;
 271
 272   char * opts = (pkt->data + ETH_HDR_LEN + ip_hdr_len + 20);
 273
 274   if (len <= 0) {
 275     return -1;
 276   }
 277
 278
 279
 280   while (offset < len) {
 281     if (*(opts + offset) == 0x00) {
 282       break;
 283     } else if (*(opts + offset) == 0x01) {
 284       offset++;
 285     } else if (*(opts + offset) == 0x08) {
 286       return offset;
 287     } else {
 288       offset += *(opts + offset + 1);
 289       //JRLDBG("Could not find timestamp\n");
 290       //break;
 291     }
 292   }
 293   return -1;
 294
 295 }
 296
 297 int compute_pkt_size(RawEthernetPacket * pkt) {
 298   if (is_ip_pkt(pkt)) {
 299     return ETH_HDR_LEN + GET_IP_TOTAL_LEN(pkt->data);
 300   }
 301
 302   return -1;
 303 }
 304
 305
 306
 307
 308 int is_arp_bcast_pkt(RawEthernetPacket * pkt) {
 309   char broadcast[MAC_LEN] = MAC_BCAST;
 310
 311   if (memcmp(ETH_DST(pkt->data), broadcast, MAC_LEN) == 0) {
 312     return 1;
 313   }
 314
 315   return 0;
 316 }
 317
 318
 319
 320 void swap_eth_addrs(RawEthernetPacket * pkt) {
 321   char mac_addr[MAC_LEN];
 322
 323   memcpy(mac_addr, ETH_DST(pkt->data), MAC_LEN);
 324
 325   // copy the source mac to dest mac
 326   memcpy(ETH_DST(pkt->data), ETH_SRC(pkt->data), MAC_LEN);
 327
 328   // set the dest mac to our taken address
 329   memcpy(ETH_SRC(pkt->data), mac_addr, MAC_LEN);
 330 }
 331
 332 void swap_ip_addrs(RawEthernetPacket * pkt) {
 333   unsigned long src_ip;
 334   unsigned long dst_ip;
 335
 336   src_ip = GET_IP_SRC(pkt->data);
 337   dst_ip = GET_IP_DST(pkt->data);
 338
 339   SET_IP_SRC(pkt->data, dst_ip);
 340   SET_IP_DST(pkt->data, src_ip);
 341 }
 342
 343 void swap_ports(RawEthernetPacket * pkt) {
 344   unsigned short src_port;
 345   unsigned short dst_port;
 346
 347   src_port = GET_TCP_SRC_PORT(pkt->data);
 348   dst_port = GET_TCP_DST_PORT(pkt->data);
 349
 350   SET_TCP_SRC_PORT(pkt->data, dst_port);
 351   SET_TCP_DST_PORT(pkt->data, src_port);
 352 }
 353
 354
 355 int is_syn_pkt(RawEthernetPacket * pkt) {
 356   char flags = GET_TCP_FLAGS(pkt->data);
 357
 358   if ((flags & TCP_SYN) == TCP_SYN) {
 359     return 1;
 360   }
 361
 362   return 0;
 363 }
 364
 365 int is_fin_pkt(RawEthernetPacket * pkt) {
 366   char flags = GET_TCP_FLAGS(pkt->data);
 367
 368
 369   if ((flags & TCP_FIN) == TCP_FIN)
 370     return 1;
 371   return 0;
 372 }
 373
 374
 375 int is_ack_pkt(RawEthernetPacket * pkt) {
 376   char flags = GET_TCP_FLAGS(pkt->data);
 377
 378    if ((flags & TCP_ACK) == TCP_ACK)
 379      return 1;
 380    return 0;
 381  }
 382
 383 int is_dns_pkt(RawEthernetPacket * pkt) {
 384   // Right now we just look at the destination port address
 385   // there is probably a better way though....
 386   if (GET_UDP_DST_PORT(pkt->data) == DNS_PORT) {
 387     return 1;
 388   }
 389   return 0;
 390 }
 391
 392 int is_tcp_pkt(RawEthernetPacket * pkt) {
 393   //int eth_hdr_len = 14;
 394   if (is_ip_pkt(pkt)) {
 395     // IP packet
 396     if (GET_IP_PROTO(pkt->data) == IP_TCP) {
 397       // TCP packet
 398       return 1;
 399     }
 400   }
 401   return 0;
 402 }
 403
 404 int is_udp_pkt(RawEthernetPacket * pkt) {
 405   if (is_ip_pkt(pkt)) {
 406     if (GET_IP_PROTO(pkt->data) == IP_UDP) {
 407       return 1;
 408     }
 409   }
 410   return 0;
 411 }
 412
 413 int is_arp_pkt(RawEthernetPacket * pkt) {
 414   if (GET_ETH_TYPE(pkt->data) == ETH_ARP) {
 415     return 1;
 416   }
 417   return 0;
 418 }
 419
 420 inline int is_ip_pkt(RawEthernetPacket * pkt) {
 421   if (GET_ETH_TYPE(pkt->data) == ETH_IP) {
 422     return 1;
 423   }
 424   return 0;
 425 }
 426
 427
 428
 429 unsigned long compute_next_tcp_seq_num(RawEthernetPacket * pkt) {
 430   if (is_syn_pkt(pkt)) {
 431     return GET_TCP_SEQ_NUM(pkt->data) + 1;
 432   } else {
 433     return GET_TCP_SEQ_NUM(pkt->data) + GET_TCP_DATA_LEN(pkt->data);
 434   }
 435
 436   return 0;
 437 }
 438
 439
 440 unsigned short compute_ip_checksum(RawEthernetPacket * pkt) {
 441   unsigned short ip_cksum;
 442   SET_IP_CKSUM(pkt->data, 0);
 443   ip_cksum =  get_ip_checksum(pkt);
 444   SET_IP_CKSUM(pkt->data, ip_cksum);
 445   return ip_cksum;
 446 }
 447
 448 unsigned short compute_tcp_checksum(RawEthernetPacket * pkt) {
 449   unsigned short tcp_cksum;
 450   SET_TCP_CKSUM(pkt->data, 0);
 451   tcp_cksum = get_tcp_checksum(pkt, GET_TCP_TOTAL_LEN(pkt->data));
 452   SET_TCP_CKSUM(pkt->data, tcp_cksum);
 453   return tcp_cksum;
 454 }
 455
 456 unsigned short compute_udp_checksum(RawEthernetPacket * pkt) {
 457   unsigned short udp_cksum;
 458   SET_UDP_CKSUM(pkt->data, 0);
 459   udp_cksum = get_udp_checksum(pkt, GET_UDP_LEN(pkt->data));
 460
 461   // Funky optional checksum... See the RFC
 462   if (udp_cksum == 0) {
 463     udp_cksum = 0xffff;
 464   }
 465   SET_UDP_CKSUM(pkt->data, udp_cksum);
 466   return udp_cksum;
 467 }
 468
 469 char * get_eth_protocol(unsigned short protocol) {
 470   if (protocol == ETH_IP) {
 471     return "IP";
 472   } else if (protocol == ETH_ARP) {
 473     return "ARP";
 474   } else if (protocol == ETH_RARP) {
 475     return "RARP";
 476   } else {
 477     return "Unknown";
 478   }
 479 }
 480
 481 char* get_ip_protocol(unsigned char protocol) {
 482   if(protocol == IP_ICMP) {
 483     return "ICMP";
 484   } else if(protocol == IP_TCP) {
 485     return "TCP";
 486   } else if(protocol == IP_UDP) {
 487     return "UDP";
 488   } else if(protocol == 121) {
 489     return "SMP";
 490   } else {
 491     return "Unknown";
 492   }
 493 }
 494
 495
 496 unsigned short get_tcp_checksum(RawEthernetPacket * pkt, unsigned short tcp_len) {
 497   unsigned short buf[1600];
 498   unsigned long  src_addr;
 499   unsigned long  dest_addr;
 500   unsigned short len;
 501   unsigned short proto;
 502
 503   len = tcp_len;
 504   len += (len % 2) ? 1 : 0;
 505
 506   src_addr = htonl(GET_IP_SRC(pkt->data));
 507   dest_addr = htonl(GET_IP_DST(pkt->data));
 508   proto = GET_IP_PROTO(pkt->data);
 509
 510   *((unsigned long *)(buf)) = src_addr;
 511   *((unsigned long *)(buf + 2)) = dest_addr;
 512
 513   buf[4]=htons(proto);
 514   buf[5]=htons(tcp_len);
 515   // return 0;
 516
 517   //  memcpy(buf + 6, (pkt->data + ETH_HDR_LEN + GET_IP_HDR_LEN(pkt->data)), tcp_len);
 518   memcpy(buf + 6, TCP_HDR(pkt->data), tcp_len);
 519   if (tcp_len % 2) {
 520     JRLDBG("Odd tcp_len: %hu\n", tcp_len);
 521     *(((char*)buf) + 2 * 6 + tcp_len) = 0;
 522   }
 523
 524   return htons(~(OnesComplementSum(buf, len/2+6)));
 525 }
 526
 527 unsigned short get_udp_checksum(RawEthernetPacket * pkt, unsigned short udp_len) {
 528   unsigned short buf[1600];
 529   unsigned long src_addr;
 530   unsigned long dest_addr;
 531   unsigned short len;
 532   unsigned short proto;
 533
 534   len = udp_len;
 535   len += (len % 2) ? 1 : 0;
 536
 537
 538   src_addr = GET_IP_SRC(pkt->data);
 539   dest_addr = GET_IP_DST(pkt->data);
 540   proto = GET_IP_PROTO(pkt->data);
 541
 542   *((unsigned long *)(buf)) = htonl(src_addr);
 543   *((unsigned long *)(buf + 2)) = htonl(dest_addr);
 544
 545   buf[4]=htons(proto);
 546   buf[5]=htons(udp_len);
 547   // return 0;
 548
 549   //  memcpy(buf + 6, (pkt->data + ETH_HDR_LEN + GET_IP_HDR_LEN(pkt->data)), udp_len);
 550   memcpy(buf + 6, UDP_HDR(pkt->data), udp_len);
 551   if (udp_len % 2) {
 552     JRLDBG("Odd udp_len: %hu\n", udp_len);
 553     *(((char*)buf) + 2 * 6 + udp_len) = 0;
 554   }
 555
 556   return htons(~(OnesComplementSum(buf, len/2+6)));
 557 }
 558
 559 unsigned short get_ip_checksum(RawEthernetPacket * pkt) {
 560   unsigned short buf[10];
 561   memset(buf, 0, 10);
 562   memcpy((char*)buf, IP_HDR(pkt->data), 20);
 563   return htons(~(OnesComplementSum(buf, 10)));
 564 }
 565
 566 unsigned short OnesComplementSum(unsigned short *buf, int len) {
 567   unsigned long sum, sum2, sum3;
 568   unsigned short realsum;
 569   int i;
 570
 571   sum=0;
 572   for (i=0;i<len;i++) {
 573     sum+=ntohs(buf[i]);
 574   }
 575   // assume there is no carry out, so now...
 576
 577   sum2 = (sum&0x0000ffff) + ((sum&0xffff0000)>>16);
 578
 579   sum3 = (sum2&0x0000ffff) +((sum2&0xffff0000)>>16);
 580
 581   realsum=sum3;
 582
 583   return realsum;
 584 }
 585
 586
 587