From 64450c2053cb52971d79c9a2351c87d9616c174d Mon Sep 17 00:00:00 2001 From: xHire Date: Wed, 7 Jan 2009 13:56:25 +0100 Subject: [PATCH] Working implementation of UDP --- wrapper/process.c | 197 +++++++++++++++++++++++++++++++++++++++++++++- wrapper/storage.c | 33 ++++++++ wrapper/storage.h | 13 +++ wrapper/wrapper.c | 4 +- wrapper/wrapper.h | 20 +++-- 5 files changed, 258 insertions(+), 9 deletions(-) diff --git a/wrapper/process.c b/wrapper/process.c index e586d66..3815a74 100644 --- a/wrapper/process.c +++ b/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(ð->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)); diff --git a/wrapper/storage.c b/wrapper/storage.c index 743fde5..4a5d60b 100644 --- a/wrapper/storage.c +++ b/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; diff --git a/wrapper/storage.h b/wrapper/storage.h index a291416..e1940d2 100644 --- a/wrapper/storage.h +++ b/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 diff --git a/wrapper/wrapper.c b/wrapper/wrapper.c index e02acae..8927948 100644 --- a/wrapper/wrapper.c +++ b/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 */ diff --git a/wrapper/wrapper.h b/wrapper/wrapper.h index 8eb3ece..4596584 100644 --- a/wrapper/wrapper.h +++ b/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); @@ -162,10 +170,10 @@ unsigned short checksum(const void *_buf, int len); unsigned short checksum_ipv6(struct in6_addr ip_src, struct in6_addr ip_dest, unsigned short paylen, unsigned char proto, unsigned char *data); /* Variables */ -extern struct s_mac_addr *mac; /* MAC address of the device */ -extern char *dev; /* capture device name */ -extern int dev_index; /* capture device index */ -extern struct in_addr *dev_ip; /* IP address associated with the device */ -extern struct in6_addr ip6addr_wrapsix; /* IPv6 prefix of WrapSix addresses */ +extern struct s_mac_addr *mac; /* MAC address of the device */ +extern char *dev; /* capture device name */ +extern int dev_index; /* capture device index */ +extern struct in_addr *dev_ip; /* IP address associated with the device */ +extern struct in6_addr ip6addr_wrapsix; /* IPv6 prefix of WrapSix addresses */ #endif