Working implementation of UDP

wrapper/process.c

@@ -62,6 +62,7 @@ void process_packet4(const struct s_ethernet *eth, const unsigned char *packet)
 			break;
 		case IPPROTO_UDP:
 			printf("   Protocol: UDP\n");
+			process_udp4(eth, ip, payload, htons(ip->pckt_len) - header_length);
 			break;
 		case IPPROTO_ICMP:
 			printf("   Protocol: ICMP\n");
@@ -73,6 +74,105 @@ void process_packet4(const struct s_ethernet *eth, const unsigned char *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;
+	struct ip6_hdr		*ip;
+	struct s_ethernet	*eth;
+
+	unsigned char	*udp_data;
+	unsigned char	*packet;
+	unsigned int	 packet_size;
+
+	struct stg_conn_udp *ent = NULL;
+	struct stg_conn_udp *ent_tmp;
+
+	/* define ICMP header */
+	udp = (struct s_udp *) payload;
+	/* define/compute ICMP 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) {
+		fprintf(stderr, "Fatal Error! Lack of free memory!\n");
+		exit(EXIT_FAILURE);
+	}
+
+	/* the only needed field is port */
+	ent_tmp->port = htons(udp->port_dest);
+
+	/* find the appropriate connection */
+	ent = jsw_rbfind(stg_conn_udp, 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_UDP,		 /* 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 */
+	udp->checksum = 0x0;
+
+	/* copy UDP header */
+	memcpy(packet + SIZE_ETHERNET + SIZE_IP6, udp, sizeof(struct s_udp));
+
+	/* copy UDP data */
+	memcpy(packet + SIZE_ETHERNET + SIZE_IP6 + sizeof(struct s_udp), udp_data, data_size - sizeof(struct s_udp));
+
+	/* compute the UDP checksum */
+	udp->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, udp, sizeof(struct s_udp));
+
+	/* send the wrapped packet back */
+	send_ipv6(packet, packet_size);
+
+	/* free allocated memory */
+	free(packet);
+}
+
 void process_icmp4(const struct s_ethernet *eth_hdr, struct s_ip4 *ip_hdr, const unsigned char *payload, unsigned short data_size)
 {
 	struct s_icmp		*icmp;
@@ -164,7 +264,7 @@ void process_icmp4(const struct s_ethernet *eth_hdr, struct s_ip4 *ip_hdr, const
 		      ent->addr_from,		 /* destination address */
 		      data_size,		 /* payload length */
 		      IPPROTO_ICMPV6,		 /* protocol */
-		      255);			 /* ttl */
+		      ip_hdr->ttl);		 /* ttl */
 
 	char ip6addr[INET6_ADDRSTRLEN];
 	inet_ntop(AF_INET6, &ent->addr_from, ip6addr, sizeof(ip6addr));
@@ -227,6 +327,7 @@ void process_packet6(const struct s_ethernet *eth, const unsigned char *packet)
 			break;
 		case IPPROTO_UDP:
 			printf("   Protocol: UDP\n");
+			process_udp6(eth, ip, payload);
 			break;
 		case IPPROTO_ICMPV6:
 			printf("   Protocol: ICMPv6\n");
@@ -238,6 +339,97 @@ void process_packet6(const struct s_ethernet *eth, const unsigned char *packet)
 	}
 }
 
+void process_udp6(const struct s_ethernet *eth, struct s_ip6 *ip, const unsigned char *payload)
+{
+	struct s_udp	*udp;
+	struct in_addr	 ip4_addr;
+
+	unsigned char	*udp_data;
+	unsigned char	*udp_packet;
+	unsigned char	 ent_save = 0;
+
+	struct stg_conn_udp *ent;
+	struct stg_conn_udp *ent_tmp;
+
+	unsigned short	 packet_size = htons(ip->len);
+
+	/* define UDP header */
+	udp = (struct s_udp *) payload;
+
+	/* define/compute UDP data offset */
+	udp_data = (unsigned char *) (payload + sizeof(struct s_udp));
+
+	/* 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) {
+		fprintf(stderr, "Fatal Error! Lack of free memory!\n");
+		exit(EXIT_FAILURE);
+	}
+
+	/* the only needed field is port */
+	ent_tmp->port = htons(udp->port_src);
+
+	/* find the appropriate connection */
+	ent = jsw_rbfind(stg_conn_udp, 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_udp *) malloc(sizeof(struct stg_conn_udp));
+		ent->port	= htons(udp->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_udp));
+		/* set fresh timestamp */
+		time(&ent->time);
+	}
+
+	/* create one big UDP packet */
+	udp_packet = (unsigned char *) malloc(packet_size);
+
+	if (udp_packet == NULL) {
+		fprintf(stderr, "Fatal error! Lack of free memory!\n");
+		exit(EXIT_FAILURE);
+	}
+
+	/* the checksum will not be computed */
+	udp->checksum = 0x0;
+
+	/* copy data into the packet */
+	memcpy(udp_packet, udp, sizeof(struct s_udp));
+	memcpy(udp_packet + sizeof(struct s_udp), udp_data, packet_size - sizeof(struct s_udp));
+
+	/* decide where to send this UDP */
+	ip4_addr = ipaddr_6to4(ip->ip_dest);
+	printf("    Send to: %s\n", inet_ntoa(ip4_addr));
+
+	/* send */
+	send_there(ip4_addr, ip->hop_limit, IPPROTO_UDP, udp_packet, packet_size);
+
+	/* save the connection */
+	if (ent_save == 1) {
+		ent->addr_to = ip4_addr;
+		jsw_rbinsert(stg_conn_udp, ent);
+		printf("     Conn #: %d\n", jsw_rbsize(stg_conn_udp));
+		/* the entry is not needed now and should be freed */
+		free(ent);
+	}
+
+	/* free allocated memory */
+	free(udp_packet);
+	udp_packet = NULL;
+}
+
 void process_icmp6(const struct s_ethernet *eth, struct s_ip6 *ip, const unsigned char *payload)
 {
 	struct s_icmp *icmp;
@@ -253,7 +445,8 @@ void process_icmp6(const struct s_ethernet *eth, struct s_ip6 *ip, const unsigne
 	unsigned short packet_size = htons(ip->len);
 
 	/* define ICMP header */
-	icmp = (struct s_icmp *) (payload);
+	icmp = (struct s_icmp *) payload;
+
 	/* define/compute ICMP data offset */
 	icmp_data = (unsigned char *) (payload + sizeof(struct s_icmp));
 

wrapper/storage.c

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

wrapper/storage.h

@@ -10,6 +10,14 @@
 #include "jsw_rbtree.h"
 #include "wrapper.h"
 
+struct stg_conn_udp {
+	unsigned short		port;
+	struct in_addr		addr_to;
+	struct in6_addr		addr_from;
+	struct s_mac_addr	mac;
+	time_t			time;
+};
+
 struct stg_conn_icmp {
 	unsigned short		id;
 	struct in_addr		addr_to;
@@ -19,10 +27,15 @@ struct stg_conn_icmp {
 	time_t			time;
 };
 
+int stg_conn_udp_cmp(const void *p1, const void *p2);
+void *stg_conn_udp_dup(void *p);
+void stg_conn_udp_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_udp;
 
 #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_udp;
 jsw_rbtree_t *stg_conn_icmp;
 
 int main(int argc, char **argv)
@@ -16,11 +17,12 @@ int main(int argc, char **argv)
 	pcap_t *handle;				/* packet capture handle */
 
 	//char filter_exp[] = "ip6";		/* filter expression */
-	char filter_exp[] = "icmp6 or icmp";	/* 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_icmp = jsw_rbnew(&stg_conn_icmp_cmp, &stg_conn_icmp_dup, &stg_conn_icmp_rel);
 
 	/* find a capture device */

wrapper/wrapper.h

@@ -85,6 +85,12 @@ struct s_tcp {
 };
 
 /* UDP structure */
+struct s_udp {
+	unsigned short	port_src;	/* 16 b; source port */
+	unsigned short	port_dest;	/* 16 b; destination port */
+	unsigned short	len;		/* 16 b; header & data length */
+	unsigned short	checksum;	/* 16 b */
+};
 
 /* ICMP header structure */
 struct s_icmp {
@@ -149,9 +155,11 @@ 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_icmp4(const struct s_ethernet *eth_hdr, struct s_ip4 *ip_hdr, const unsigned char *payload, unsigned short packet_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_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);