Implemented TCP, but it's not working yet

wrapper/checksum.c

@@ -31,6 +31,37 @@ unsigned short checksum(const void *_buf, int len)
 	return ~sum;
 }
 
+unsigned short checksum_ipv4(struct in_addr ip_src, struct in_addr ip_dest, unsigned short length, unsigned char proto, unsigned char *data)
+{
+	unsigned char		*buf_ip4_pseudo;
+	struct s_ip4_pseudo	*ip4_pseudo;
+	unsigned short		 sum;
+
+	buf_ip4_pseudo = (unsigned char *) malloc(sizeof(struct s_ip4_pseudo) + length);
+
+	if (buf_ip4_pseudo == NULL) {
+		fprintf(stderr, "Fatal error! Lack of free memory!\n");
+		exit(EXIT_FAILURE);
+	}
+
+	ip4_pseudo = (struct s_ip4_pseudo *) buf_ip4_pseudo;
+
+	ip4_pseudo->ip_src	= ip_src;
+	ip4_pseudo->ip_dest	= ip_dest;
+	ip4_pseudo->len		= htons(length);
+	ip4_pseudo->zeros	= 0x0;
+	ip4_pseudo->proto	= proto;
+
+	memcpy(buf_ip4_pseudo + sizeof(struct s_ip4_pseudo), data, length);
+
+	sum = checksum(buf_ip4_pseudo, sizeof(struct s_ip4_pseudo) + length);
+
+	free(buf_ip4_pseudo);
+	buf_ip4_pseudo = NULL;
+
+	return sum;
+}
+
 unsigned short checksum_ipv6(struct in6_addr ip_src, struct in6_addr ip_dest, unsigned short paylen, unsigned char proto, unsigned char *data)
 {
 	unsigned char		*buf_ip6_pseudo;

wrapper/process.c

@@ -59,6 +59,7 @@ void process_packet4(const struct s_ethernet *eth, const unsigned char *packet)
 	switch (ip->proto) {
 		case IPPROTO_TCP:
 			printf("   Protocol: TCP\n");
+			process_tcp4(eth, ip, payload, htons(ip->pckt_len) - header_length);
 			break;
 		case IPPROTO_UDP:
 			printf("   Protocol: UDP\n");
@@ -74,6 +75,99 @@ void process_packet4(const struct s_ethernet *eth, const unsigned char *packet)
 	}
 }
 
+void process_tcp4(const struct s_ethernet *eth_hdr, struct s_ip4 *ip_hdr, const unsigned char *payload, unsigned short data_size)
+{
+	struct s_tcp		*tcp;
+	struct ip6_hdr		*ip;
+	struct s_ethernet	*eth;
+
+	unsigned char	*packet;
+	unsigned int	 packet_size;
+
+	struct stg_conn_tup *ent = NULL;
+	struct stg_conn_tup *ent_tmp;
+
+	/* define TCP header */
+	tcp = (struct s_tcp *) payload;
+
+	/* create temporary data entry for finding */
+	if ((ent_tmp = (struct stg_conn_tup *) malloc(sizeof(struct stg_conn_tup))) == NULL) {
+		fprintf(stderr, "Fatal Error! Lack of free memory!\n");
+		exit(EXIT_FAILURE);
+	}
+
+	/* the only needed field is port */
+	ent_tmp->port = htons(tcp->port_dest);
+
+	/* find the appropriate connection */
+	ent = jsw_rbfind(stg_conn_tcp, ent_tmp);
+
+	/* free allocated memory */
+	free(ent_tmp);
+
+	/* check if this packet is from wrapped connection */
+	if (ent == NULL) {
+		fprintf(stderr, "Error: data not found\n");
+		return;
+	}
+	else if (memcmp(&ent->addr_to, &ip_hdr->ip_src, sizeof(struct in_addr))) {
+		fprintf(stderr, "Error: data not appropriate\n");
+		printf("     Ent-to: %s\n", inet_ntoa(ent->addr_to));
+		printf("    IP-from: %s\n", inet_ntoa(ip_hdr->ip_src));
+		return;
+	}
+
+	packet_size = data_size + SIZE_ETHERNET + SIZE_IP6;
+	packet = (unsigned char *) malloc(packet_size);
+
+	if (packet == NULL) {
+		fprintf(stderr, "Fatal error! Lack of free memory!\n");
+		exit(EXIT_FAILURE);
+	}
+
+	/* initialize the packet with zeros */
+	memset(packet, 0x0, packet_size);
+
+	/* parse the packet into structures */
+	eth	= (struct s_ethernet *)	packet;
+	ip	= (struct ip6_hdr *)	(packet + SIZE_ETHERNET);
+
+	/* assemble the ethernet header */
+	memcpy(&eth->src, mac, sizeof(struct s_mac_addr));
+	eth->dest = ent->mac;
+	eth->type = htons(ETHERTYPE_IPV6);
+
+	/* assemble the IPv6 header */
+	build_ip6_hdr(ip,			 /* ip6_hdr structure */
+		      ipaddr_4to6(ent->addr_to), /* source address */
+		      ent->addr_from,		 /* destination address */
+		      data_size,		 /* payload length */
+		      IPPROTO_TCP,		 /* protocol */
+		      ip_hdr->ttl);		 /* ttl */
+
+	char ip6addr[INET6_ADDRSTRLEN];
+	inet_ntop(AF_INET6, &ent->addr_from, ip6addr, sizeof(ip6addr));
+	printf("    Send to: %s\n", ip6addr);
+
+	/* set the checksum to zero */
+	tcp->checksum = 0x0;
+
+	/* copy data into the packet */
+	memcpy(packet + SIZE_ETHERNET + SIZE_IP6, payload, data_size);
+
+	/* compute the TCP checksum */
+	tcp->checksum = checksum_ipv6(ip->ip6_src, ip->ip6_dst, data_size, ip->ip6_nxt, (unsigned char *) (packet + SIZE_ETHERNET + SIZE_IP6));
+
+	/* return the checksum into the packet */
+	memcpy(packet + SIZE_ETHERNET + SIZE_IP6, tcp, sizeof(struct s_tcp));
+
+	/* send the wrapped packet back */
+	send_ipv6(packet, packet_size);
+
+	/* free allocated memory */
+	free(packet);
+}
+
 void process_udp4(const struct s_ethernet *eth_hdr, struct s_ip4 *ip_hdr, const unsigned char *payload, unsigned short data_size)
 {
 	struct s_udp		*udp;
@@ -84,16 +178,16 @@ void process_udp4(const struct s_ethernet *eth_hdr, struct s_ip4 *ip_hdr, const
 	unsigned char	*packet;
 	unsigned int	 packet_size;
 
-	struct stg_conn_udp *ent = NULL;
+	struct stg_conn_tup *ent = NULL;
-	struct stg_conn_udp *ent_tmp;
+	struct stg_conn_tup *ent_tmp;
 
-	/* define ICMP header */
+	/* define UDP header */
 	udp = (struct s_udp *) payload;
-	/* define/compute ICMP data offset */
+	/* define/compute UDP data offset */
 	udp_data = (unsigned char *) (payload + sizeof(struct s_udp));
 
 	/* create temporary data entry for finding */
-	if ((ent_tmp = (struct stg_conn_udp *) malloc(sizeof(struct stg_conn_udp))) == NULL) {
+	if ((ent_tmp = (struct stg_conn_tup *) malloc(sizeof(struct stg_conn_tup))) == NULL) {
 		fprintf(stderr, "Fatal Error! Lack of free memory!\n");
 		exit(EXIT_FAILURE);
 	}
@@ -324,6 +418,7 @@ void process_packet6(const struct s_ethernet *eth, const unsigned char *packet)
 	switch (ip->next_header) {
 		case IPPROTO_TCP:
 			printf("   Protocol: TCP\n");
+			process_tcp6(eth, ip, payload);
 			break;
 		case IPPROTO_UDP:
 			printf("   Protocol: UDP\n");
@@ -339,6 +434,99 @@ void process_packet6(const struct s_ethernet *eth, const unsigned char *packet)
 	}
 }
 
+void process_tcp6(const struct s_ethernet *eth, struct s_ip6 *ip, const unsigned char *payload)
+{
+	struct s_tcp	*tcp;
+	struct in_addr	 ip4_addr_src, ip4_addr_dest;
+
+	unsigned char	*tcp_packet;
+	unsigned char	 ent_save = 0;
+
+	struct stg_conn_tup *ent;
+	struct stg_conn_tup *ent_tmp;
+
+	unsigned short	 packet_size = htons(ip->len);
+
+	/* define TCP header */
+	tcp = (struct s_tcp *) payload;
+
+	/* check whether the connection is not already saved */
+	/* create temporary data entry for finding */
+	if ((ent_tmp = (struct stg_conn_tup *) malloc(sizeof(struct stg_conn_tup))) == NULL) {
+		fprintf(stderr, "Fatal Error! Lack of free memory!\n");
+		exit(EXIT_FAILURE);
+	}
+
+	/* the only needed field is port */
+	ent_tmp->port = htons(tcp->port_src);
+
+	/* find the appropriate connection */
+	ent = jsw_rbfind(stg_conn_tcp, ent_tmp);
+
+	/* free allocated memory */
+	free(ent_tmp);
+
+	/* check if this packet is from wrapped connection */
+	if (ent == NULL) {
+		printf("New connection\n");
+		/* save the connection */
+		ent = (struct stg_conn_tup *) malloc(sizeof(struct stg_conn_tup));
+		ent->port	= htons(tcp->port_src);
+		ent->addr_from	= ip->ip_src;
+		ent->mac	= eth->src;
+		time(&ent->time);
+		memset(&ent->addr_to, 0x0, sizeof(struct in_addr));
+		ent_save = 1;
+	}
+	else {
+		printf("Connection found\n");
+		printf("     Conn #: %d\n", jsw_rbsize(stg_conn_tcp));
+		/* set fresh timestamp */
+		time(&ent->time);
+	}
+
+	/* decide where to send this TCP */
+	ip4_addr_dest = ipaddr_6to4(ip->ip_dest);
+	printf("    Send to: %s\n", inet_ntoa(ip4_addr_dest));
+
+	/* create one big TCP packet */
+	tcp_packet = (unsigned char *) malloc(packet_size);
+
+	if (tcp_packet == NULL) {
+		fprintf(stderr, "Fatal error! Lack of free memory!\n");
+		exit(EXIT_FAILURE);
+	}
+
+	/* first set the checksum to zero */
+	tcp->checksum = 0x0;
+
+	/* copy data into the packet */
+	memcpy(tcp_packet, payload, packet_size);
+
+	/* compute the checksum */
+	memcpy(&ip4_addr_src, dev_ip, sizeof(struct in_addr));
+	tcp->checksum = checksum_ipv4(ip4_addr_src, ip4_addr_dest, packet_size, IPPROTO_TCP, tcp_packet);
+
+	/* copy this structure again - because of the checksum */
+	memcpy(tcp_packet, tcp, sizeof(struct s_tcp));
+
+	/* send */
+	send_there(ip4_addr_dest, ip->hop_limit, IPPROTO_TCP, tcp_packet, packet_size);
+
+	/* save the connection */
+	if (ent_save == 1) {
+		ent->addr_to = ip4_addr_dest;
+		jsw_rbinsert(stg_conn_tcp, ent);
+		printf("     Conn #: %d\n", jsw_rbsize(stg_conn_tcp));
+		/* the entry is not needed now and should be freed */
+		free(ent);
+	}
+
+	/* free allocated memory */
+	free(tcp_packet);
+	tcp_packet = NULL;
+}
+
 void process_udp6(const struct s_ethernet *eth, struct s_ip6 *ip, const unsigned char *payload)
 {
 	struct s_udp	*udp;
@@ -348,8 +536,8 @@ void process_udp6(const struct s_ethernet *eth, struct s_ip6 *ip, const unsigned
 	unsigned char	*udp_packet;
 	unsigned char	 ent_save = 0;
 
-	struct stg_conn_udp *ent;
+	struct stg_conn_tup *ent;
-	struct stg_conn_udp *ent_tmp;
+	struct stg_conn_tup *ent_tmp;
 
 	unsigned short	 packet_size = htons(ip->len);
 
@@ -361,7 +549,7 @@ void process_udp6(const struct s_ethernet *eth, struct s_ip6 *ip, const unsigned
 
 	/* check whether the connection is not already saved */
 	/* create temporary data entry for finding */
-	if ((ent_tmp = (struct stg_conn_udp *) malloc(sizeof(struct stg_conn_udp))) == NULL) {
+	if ((ent_tmp = (struct stg_conn_tup *) malloc(sizeof(struct stg_conn_tup))) == NULL) {
 		fprintf(stderr, "Fatal Error! Lack of free memory!\n");
 		exit(EXIT_FAILURE);
 	}
@@ -379,7 +567,7 @@ void process_udp6(const struct s_ethernet *eth, struct s_ip6 *ip, const unsigned
 	if (ent == NULL) {
 		printf("New connection\n");
 		/* save the connection */
-		ent = (struct stg_conn_udp *) malloc(sizeof(struct stg_conn_udp));
+		ent = (struct stg_conn_tup *) malloc(sizeof(struct stg_conn_tup));
 		ent->port	= htons(udp->port_src);
 		ent->addr_from	= ip->ip_src;
 		ent->mac	= eth->src;
@@ -528,7 +716,7 @@ void process_icmp6(const struct s_ethernet *eth, struct s_ip6 *ip, const unsigne
 	}
 
 	/* the checksum has to be zeros before we have data for its computation */
-	icmp->checksum = 0;
+	icmp->checksum = 0x0;
 
 	/* copy data into the packet */
 	memcpy(icmp_packet, icmp, sizeof(struct s_icmp));

wrapper/storage.c

@@ -1,32 +1,32 @@
 #include "storage.h"
 
-/*** UDP ***/
+/*** TCP & UDP ***/
-int stg_conn_udp_cmp(const void *p1, const void *p2)
+int stg_conn_tup_cmp(const void *p1, const void *p2)
 {
-	struct stg_conn_udp *pp1 = (struct stg_conn_udp *) p1;
+	struct stg_conn_tup *pp1 = (struct stg_conn_tup *) p1;
-	struct stg_conn_udp *pp2 = (struct stg_conn_udp *) p2;
+	struct stg_conn_tup *pp2 = (struct stg_conn_tup *) p2;
 
 	if (pp1->port < pp2->port) return -1;
 	if (pp1->port > pp2->port) return  1;
 	return 0;
 }
 
-void *stg_conn_udp_dup(void *p)
+void *stg_conn_tup_dup(void *p)
 {
-	struct stg_conn_udp *pp = (struct stg_conn_udp *) p;
+	struct stg_conn_tup *pp = (struct stg_conn_tup *) p;
-	struct stg_conn_udp *p_new;
+	struct stg_conn_tup *p_new;
 
-	if ((p_new = (struct stg_conn_udp *) malloc(sizeof(struct stg_conn_udp))) == NULL) {
+	if ((p_new = (struct stg_conn_tup *) malloc(sizeof(struct stg_conn_tup))) == NULL) {
 		fprintf(stderr, "Fatal Error! Lack of free memory!\n");
 		exit(EXIT_FAILURE);
 	}
 
-	memcpy(p_new, pp, sizeof(struct stg_conn_udp));
+	memcpy(p_new, pp, sizeof(struct stg_conn_tup));
 
 	return (void *) p_new;
 }
 
-void stg_conn_udp_rel(void *p)
+void stg_conn_tup_rel(void *p)
 {
 	free(p);
 	p = NULL;

wrapper/storage.h

@@ -10,7 +10,7 @@
 #include "jsw_rbtree.h"
 #include "wrapper.h"
 
-struct stg_conn_udp {
+struct stg_conn_tup {
 	unsigned short		port;
 	struct in_addr		addr_to;
 	struct in6_addr		addr_from;
@@ -27,15 +27,16 @@ struct stg_conn_icmp {
 	time_t			time;
 };
 
-int stg_conn_udp_cmp(const void *p1, const void *p2);
+int stg_conn_tup_cmp(const void *p1, const void *p2);
-void *stg_conn_udp_dup(void *p);
+void *stg_conn_tup_dup(void *p);
-void stg_conn_udp_rel(void *p);
+void stg_conn_tup_rel(void *p);
 
 int stg_conn_icmp_cmp(const void *p1, const void *p2);
 void *stg_conn_icmp_dup(void *p);
 void stg_conn_icmp_rel(void *p);
 
-extern jsw_rbtree_t *stg_conn_icmp;
+extern jsw_rbtree_t *stg_conn_tcp;
 extern jsw_rbtree_t *stg_conn_udp;
+extern jsw_rbtree_t *stg_conn_icmp;
 
 #endif

wrapper/wrapper.c

@@ -7,6 +7,7 @@ int  dev_index;				/* capture device index */
 struct in_addr *dev_ip;			/* IP address associated with the device */
 
 /* storage trees */
+jsw_rbtree_t *stg_conn_tcp;
 jsw_rbtree_t *stg_conn_udp;
 jsw_rbtree_t *stg_conn_icmp;
 
@@ -16,13 +17,13 @@ int main(int argc, char **argv)
 	char errbuf[PCAP_ERRBUF_SIZE];		/* error buffer */
 	pcap_t *handle;				/* packet capture handle */
 
-	//char filter_exp[] = "ip6";		/* filter expression */
+	char filter_exp[] = "";			/* filter expression */
-	char filter_exp[] = "icmp6 or icmp or udp";	/* filter expression */
 	struct bpf_program fp;			/* compiled filter program (expression) */
 	int num_packets = 0;			/* number of packets to capture; 0 = infinite */
 
 	/* initialize the storage for connections */
-	stg_conn_udp  = jsw_rbnew(&stg_conn_udp_cmp, &stg_conn_udp_dup, &stg_conn_udp_rel);
+	stg_conn_tcp  = jsw_rbnew(&stg_conn_tup_cmp, &stg_conn_tup_dup, &stg_conn_tup_rel);
+	stg_conn_udp  = jsw_rbnew(&stg_conn_tup_cmp, &stg_conn_tup_dup, &stg_conn_tup_rel);
 	stg_conn_icmp = jsw_rbnew(&stg_conn_icmp_cmp, &stg_conn_icmp_dup, &stg_conn_icmp_rel);
 
 	/* find a capture device */
@@ -87,6 +88,8 @@ int main(int argc, char **argv)
 	pcap_freecode(&fp);
 	pcap_close(handle);
 
+	jsw_rbdelete(stg_conn_tcp);
+	jsw_rbdelete(stg_conn_udp);
 	jsw_rbdelete(stg_conn_icmp);
 
 	free(mac);

wrapper/wrapper.h

@@ -44,7 +44,7 @@ struct s_ethernet {
 struct s_ip4 {
 	unsigned char	ver_ihl;
 	unsigned char	tos;		/*  8 b; type of service */
-	unsigned short	pckt_len;	/* 16 b; total lenght of the packet (IP header + payload) */
+	unsigned short	pckt_len;	/* 16 b; total length of the packet (IP header + payload) */
 	unsigned short	id;		/* 16 b; id of the packet - for purpose of fragmentation */
 	unsigned short	flags_offset;	/* 16 b; 3 b - flags, 13 b - fragment offset in bytes */
 	unsigned char	ttl;		/*  8 b; time to live */
@@ -54,6 +54,15 @@ struct s_ip4 {
 	struct in_addr	ip_dest;	/* 32 b; destination address */
 };
 
+/* pseudo IPv4 header for checksum */
+struct s_ip4_pseudo {
+	struct in_addr	ip_src;		/* 32 b; source address */
+	struct in_addr	ip_dest;	/* 32 b; destination address */
+	unsigned char	zeros;		/*  8 b */
+	unsigned char	proto;		/*  8 b; protocol in the payload */
+	unsigned short	len;		/* 16 b; payload length */
+};
+
 /* IPv6 header structure */
 struct s_ip6 {
 	unsigned char	ver;		/*   8 b; version */
@@ -75,13 +84,17 @@ struct s_ip6_pseudo {
 	unsigned char	next_header;	/*   8 b; next header */
 };
 
-/* TCP structure - only needed fields! */
+/* TCP structure */
 struct s_tcp {
 	unsigned short	port_src;	/* 16 b; source port */
 	unsigned short	port_dest;	/* 16 b; destination port */
-	long double	data1;		/* 96 b; first data segment */
+	unsigned int	seq;		/* 32 b; sequence number */
+	unsigned int	ack;		/* 32 b; acknowledgement number */
+	unsigned char	dataoff_res;	/*  8 b; 4 b - data offset, 4 b - reserved (zeros) */
+	unsigned char	flags;		/*  8 b; 8 flags */
+	unsigned short	windows_size;	/* 16 b; size of the receive window */
 	unsigned short	checksum;	/* 16 b */
-	unsigned short	data2;		/* 16 b; the rest (urgent pointer here) */
+	unsigned short	urgent_pointer;	/* 16 b; indicates last urgent data byte */
 };
 
 /* UDP structure */
@@ -155,10 +168,12 @@ int get_dev_index(const char *dev);
 void process_packet(u_char *args, const struct pcap_pkthdr *header, const u_char *packet);
 
 void process_packet4(const struct s_ethernet *eth, const unsigned char *packet);
+void process_tcp4(const struct s_ethernet *eth_hdr, struct s_ip4 *ip_hdr, const unsigned char *payload, unsigned short data_size);
 void process_udp4(const struct s_ethernet *eth_hdr, struct s_ip4 *ip_hdr, const unsigned char *payload, unsigned short data_size);
 void process_icmp4(const struct s_ethernet *eth_hdr, struct s_ip4 *ip_hdr, const unsigned char *payload, unsigned short data_size);
 
 void process_packet6(const struct s_ethernet *eth, const unsigned char *packet);
+void process_tcp6(const struct s_ethernet *eth, struct s_ip6 *ip, const unsigned char *payload);
 void process_udp6(const struct s_ethernet *eth, struct s_ip6 *ip, const unsigned char *payload);
 void process_icmp6(const struct s_ethernet *eth, struct s_ip6 *ip, const unsigned char *payload);
 void process_ndp(const struct s_ethernet *eth_hdr, struct s_ip6 *ip_hdr, unsigned char *icmp_data);
@@ -167,6 +182,7 @@ void send_there(struct in_addr ip4_addr, unsigned char ttl, unsigned int type, u
 void send_ipv6(unsigned char *packet, int packet_size);
 
 unsigned short checksum(const void *_buf, int len);
+unsigned short checksum_ipv4(struct in_addr ip_src, struct in_addr ip_dest, unsigned short length, unsigned char proto, unsigned char *data);
 unsigned short checksum_ipv6(struct in6_addr ip_src, struct in6_addr ip_dest, unsigned short paylen, unsigned char proto, unsigned char *data);
 
 /* Variables */