5 void dbg_print_pkt_info(RawEthernetPacket * pkt) {
6 unsigned short src_port;
7 unsigned short dest_port;
11 unsigned long seq_num = GET_TCP_SEQ_NUM(pkt->data);
12 unsigned long ack_num = GET_TCP_ACK_NUM(pkt->data);
14 src_port = GET_TCP_SRC_PORT(pkt->data);
15 dest_port = GET_TCP_DST_PORT(pkt->data);
17 dest_str = ip_to_string(GET_IP_DST(pkt->data));
18 src_str = ip_to_string(GET_IP_SRC(pkt->data));
20 vtl_debug("Packet: %s:%d-%s:%d seq: %lu, ack: %lu\n", src_str.c_str(), ntohs(src_port), dest_str.c_str(), ntohs(dest_port),
27 void dbg_print_pkt(RawEthernetPacket * pkt) {
31 unsigned int pkt_size = pkt->get_size() - 1;
33 vtl_debug("Packet Dump: (pkt_size=%lu) \n", pkt->get_size());
35 for (x = 0; x < pkt_size;) {
36 sprintf(pkt_line, "\t%.4x: ", x);
38 for (i = 0; i < 16; i += 2) {
39 if (pkt_size < x + i) {
43 if (pkt_size == x + i) {
44 sprintf(pkt_line, "%s%.2x ", pkt_line, *(unsigned char *)(pkt->data + i + x));
47 sprintf(pkt_line, "%s%.4x ", pkt_line, ntohs(*(unsigned short *)(pkt->data + i + x)));
51 vtl_debug("%s\n", pkt_line);
57 void dbg_print_buf(unsigned char * buf, unsigned int len) {
62 vtl_debug("Buf Dump: (len=%d) \n", len);
64 for (x = 0; x < len-1;) {
65 sprintf(pkt_line, "\t%.4x: ", x);
67 for (i = 0; i < 16; i += 2) {
68 if ((len - 1) < x + i) {
72 if (len == x + i + 1) {
73 sprintf(pkt_line, "%s%.2x ", pkt_line, *(unsigned char *)(buf + i + x));
76 sprintf(pkt_line, "%s%.4x ", pkt_line, ntohs(*(unsigned short *)(buf + i + x)));
80 vtl_debug("%s\n", pkt_line);
87 void do_binary_to_ipaddress(unsigned char* ip,IPADDRESS& ipaddress)
95 void do_ipaddress_to_string(IPADDRESS ipaddress,char* buffer)
97 sprintf(buffer,"%d.%d.%d.%d",ipaddress.a1,ipaddress.a2,ipaddress.a3,ipaddress.a4);
100 void do_binary_to_string(unsigned char* ip,char* buffer)
103 do_binary_to_ipaddress(ip,ipaddress);
104 do_ipaddress_to_string(ipaddress,buffer);
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;
112 int len = tcp_hdr_len - 20;
115 char * opts = (pkt->data + ETH_HDR_LEN + ip_hdr_len + 20);
121 while (offset < len) {
122 if (*(opts + offset) == 0x00) {
124 } else if (*(opts + offset) == 0x01) {
126 } else if (*(opts + offset) == 0x02) {
127 mss = (*(unsigned short *)(opts + offset + 2));
128 offset += *(opts + offset + 1);
129 return (int)ntohs(mss);
131 offset += *(opts + offset + 1);
137 int parse_tcp_options(tcp_opts_t * options, RawEthernetPacket * pkt) {
138 assert((options != NULL) && (pkt != NULL));
140 memset(options, 0, sizeof(options));
143 char * opts = TCP_OPTS(pkt->data);
144 int opt_len = GET_TCP_OPTS_LEN(pkt->data);
152 while (offset < opt_len) {
153 if (*(opts + offset) == 0x00) {
155 } else if (*(opts + offset) == 0x01) {
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) {
165 options->sack_ok = 1;
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);
172 } else if (*(opts + offset) == 0x08) {
174 options->local_ts = *(unsigned long *)(opts + offset);
176 options->remote_ts = *(unsigned long *)(opts + offset);
179 // default handler to skip what we don't look for
180 offset += *(opts + offset + 1);
186 int set_tcp_options(tcp_opts_t * options, unsigned long opt_flags, RawEthernetPacket * pkt) {
187 char * pkt_opts = TCP_DATA(pkt->data);
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);
199 *(pkt_opts + offset) = 0x00;
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);
211 unsigned long get_tcp_timestamp(char *opts, int len) {
213 unsigned long timestamp = 0;
214 unsigned long * ts_ptr;
216 while (offset < len) {
217 if (*(opts + offset) == 0x00) {
219 } else if (*(opts + offset) == 0x01) {
221 } else if (*(opts + offset) == 0x08) {
223 ts_ptr = (unsigned long *)(opts + offset);
224 timestamp = (*ts_ptr);
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) {
233 } else if (*(opts + offset) == 0x05) {
234 offset += *(opts + offset + 1);
236 offset += *(opts + offset + 1);
237 //vtl_debug("Could not find timestamp\n");
244 void set_tcp_timestamp(char * ts_opt, unsigned long local_ts, unsigned long remote_ts) {
247 // *(ts_opt + offset) = 0x01;
249 //*(ts_opt + offset) = 0x01;
251 *(ts_opt + offset) = 0x08;
253 *(ts_opt + offset) = 0x0a;
256 *(unsigned long *)(ts_opt + offset) = local_ts;
257 offset += sizeof(unsigned long);
259 *(unsigned long *)(ts_opt + offset) = remote_ts;
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;
270 int len = tcp_hdr_len - 20;
272 char * opts = (pkt->data + ETH_HDR_LEN + ip_hdr_len + 20);
280 while (offset < len) {
281 if (*(opts + offset) == 0x00) {
283 } else if (*(opts + offset) == 0x01) {
285 } else if (*(opts + offset) == 0x08) {
288 offset += *(opts + offset + 1);
289 //vtl_debug("Could not find timestamp\n");
297 int compute_pkt_size(RawEthernetPacket * pkt) {
298 if (is_ip_pkt(pkt)) {
299 return ETH_HDR_LEN + GET_IP_TOTAL_LEN(pkt->data);
308 int is_arp_bcast_pkt(RawEthernetPacket * pkt) {
309 char broadcast[MAC_LEN] = MAC_BCAST;
311 if (memcmp(ETH_DST(pkt->data), broadcast, MAC_LEN) == 0) {
320 void swap_eth_addrs(RawEthernetPacket * pkt) {
321 char mac_addr[MAC_LEN];
323 memcpy(mac_addr, ETH_DST(pkt->data), MAC_LEN);
325 // copy the source mac to dest mac
326 memcpy(ETH_DST(pkt->data), ETH_SRC(pkt->data), MAC_LEN);
328 // set the dest mac to our taken address
329 memcpy(ETH_SRC(pkt->data), mac_addr, MAC_LEN);
332 void swap_ip_addrs(RawEthernetPacket * pkt) {
333 unsigned long src_ip;
334 unsigned long dst_ip;
336 src_ip = GET_IP_SRC(pkt->data);
337 dst_ip = GET_IP_DST(pkt->data);
339 SET_IP_SRC(pkt->data, dst_ip);
340 SET_IP_DST(pkt->data, src_ip);
343 void swap_ports(RawEthernetPacket * pkt) {
344 unsigned short src_port;
345 unsigned short dst_port;
347 src_port = GET_TCP_SRC_PORT(pkt->data);
348 dst_port = GET_TCP_DST_PORT(pkt->data);
350 SET_TCP_SRC_PORT(pkt->data, dst_port);
351 SET_TCP_DST_PORT(pkt->data, src_port);
355 int is_syn_pkt(RawEthernetPacket * pkt) {
356 char flags = GET_TCP_FLAGS(pkt->data);
358 if ((flags & TCP_SYN) == TCP_SYN) {
365 int is_fin_pkt(RawEthernetPacket * pkt) {
366 char flags = GET_TCP_FLAGS(pkt->data);
369 if ((flags & TCP_FIN) == TCP_FIN)
375 int is_ack_pkt(RawEthernetPacket * pkt) {
376 char flags = GET_TCP_FLAGS(pkt->data);
378 if ((flags & TCP_ACK) == TCP_ACK)
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) {
392 int is_tcp_pkt(RawEthernetPacket * pkt) {
393 //int eth_hdr_len = 14;
394 if (is_ip_pkt(pkt)) {
396 if (GET_IP_PROTO(pkt->data) == IP_TCP) {
404 int is_udp_pkt(RawEthernetPacket * pkt) {
405 if (is_ip_pkt(pkt)) {
406 if (GET_IP_PROTO(pkt->data) == IP_UDP) {
413 int is_arp_pkt(RawEthernetPacket * pkt) {
414 if (GET_ETH_TYPE(pkt->data) == ETH_ARP) {
420 inline int is_ip_pkt(RawEthernetPacket * pkt) {
421 if (GET_ETH_TYPE(pkt->data) == ETH_IP) {
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;
433 return GET_TCP_SEQ_NUM(pkt->data) + GET_TCP_DATA_LEN(pkt->data);
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);
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);
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));
461 // Funky optional checksum... See the RFC
462 if (udp_cksum == 0) {
465 SET_UDP_CKSUM(pkt->data, udp_cksum);
469 char * get_eth_protocol(unsigned short protocol) {
470 if (protocol == ETH_IP) {
472 } else if (protocol == ETH_ARP) {
474 } else if (protocol == ETH_RARP) {
481 char* get_ip_protocol(unsigned char protocol) {
482 if(protocol == IP_ICMP) {
484 } else if(protocol == IP_TCP) {
486 } else if(protocol == IP_UDP) {
488 } else if(protocol == 121) {
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;
501 unsigned short proto;
504 len += (len % 2) ? 1 : 0;
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);
510 *((unsigned long *)(buf)) = src_addr;
511 *((unsigned long *)(buf + 2)) = dest_addr;
514 buf[5]=htons(tcp_len);
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);
520 vtl_debug("Odd tcp_len: %hu\n", tcp_len);
521 *(((char*)buf) + 2 * 6 + tcp_len) = 0;
524 return htons(~(OnesComplementSum(buf, len/2+6)));
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;
532 unsigned short proto;
535 len += (len % 2) ? 1 : 0;
538 src_addr = GET_IP_SRC(pkt->data);
539 dest_addr = GET_IP_DST(pkt->data);
540 proto = GET_IP_PROTO(pkt->data);
542 *((unsigned long *)(buf)) = htonl(src_addr);
543 *((unsigned long *)(buf + 2)) = htonl(dest_addr);
546 buf[5]=htons(udp_len);
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);
552 vtl_debug("Odd udp_len: %hu\n", udp_len);
553 *(((char*)buf) + 2 * 6 + udp_len) = 0;
556 return htons(~(OnesComplementSum(buf, len/2+6)));
559 unsigned short get_ip_checksum(RawEthernetPacket * pkt) {
560 unsigned short buf[10];
562 memcpy((char*)buf, IP_HDR(pkt->data), 20);
563 return htons(~(OnesComplementSum(buf, 10)));
566 unsigned short OnesComplementSum(unsigned short *buf, int len) {
567 unsigned long sum, sum2, sum3;
568 unsigned short realsum;
572 for (i=0;i<len;i++) {
575 // assume there is no carry out, so now...
577 sum2 = (sum&0x0000ffff) + ((sum&0xffff0000)>>16);
579 sum3 = (sum2&0x0000ffff) +((sum2&0xffff0000)>>16);