diff --git a/openair-cn/GTPV1-U/GTPURH/xt_GTPURH.c b/openair-cn/GTPV1-U/GTPURH/xt_GTPURH.c index 6281363585..f3f0af0c0d 100755 --- a/openair-cn/GTPV1-U/GTPURH/xt_GTPURH.c +++ b/openair-cn/GTPV1-U/GTPURH/xt_GTPURH.c @@ -39,6 +39,220 @@ MODULE_AUTHOR("Pradip Biswas "); MODULE_DESCRIPTION("GTPu Data Path extension on netfilter"); +/* + * Statically sized hash table implementation + * (C) 2012 Sasha Levin + */ + +#ifndef _LINUX_HASHTABLE_H +#define _LINUX_HASHTABLE_H + +#include +#include +#include +#include +#include + +#define DEFINE_HASHTABLE(name, bits) \ + struct hlist_head name[1 << (bits)] = \ + { [0 ... ((1 << (bits)) - 1)] = HLIST_HEAD_INIT } + +#define DECLARE_HASHTABLE(name, bits) \ + struct hlist_head name[1 << (bits)] + +#define HASH_SIZE(name) (ARRAY_SIZE(name)) +#define HASH_BITS(name) ilog2(HASH_SIZE(name)) + +/* Use hash_32 when possible to allow for fast 32bit hashing in 64bit kernels. */ +#define hash_min(val, bits) \ + (sizeof(val) <= 4 ? hash_32(val, bits) : hash_long(val, bits)) + +static inline void __hash_init(struct hlist_head *ht, unsigned int sz) +{ + unsigned int i; + + for (i = 0; i < sz; i++) + INIT_HLIST_HEAD(&ht[i]); +} + +/** + * hash_init - initialize a hash table + * @hashtable: hashtable to be initialized + * + * Calculates the size of the hashtable from the given parameter, otherwise + * same as hash_init_size. + * + * This has to be a macro since HASH_BITS() will not work on pointers since + * it calculates the size during preprocessing. + */ +#define hash_init(hashtable) __hash_init(hashtable, HASH_SIZE(hashtable)) + +/** + * hash_add - add an object to a hashtable + * @hashtable: hashtable to add to + * @node: the &struct hlist_node of the object to be added + * @key: the key of the object to be added + */ +#define hash_add(hashtable, node, key) \ + hlist_add_head(node, &hashtable[hash_min(key, HASH_BITS(hashtable))]) + +/** + * hash_add_rcu - add an object to a rcu enabled hashtable + * @hashtable: hashtable to add to + * @node: the &struct hlist_node of the object to be added + * @key: the key of the object to be added + */ +#define hash_add_rcu(hashtable, node, key) \ + hlist_add_head_rcu(node, &hashtable[hash_min(key, HASH_BITS(hashtable))]) + +/** + * hash_hashed - check whether an object is in any hashtable + * @node: the &struct hlist_node of the object to be checked + */ +static inline bool hash_hashed(struct hlist_node *node) +{ + return !hlist_unhashed(node); +} + +static inline bool __hash_empty(struct hlist_head *ht, unsigned int sz) +{ + unsigned int i; + + for (i = 0; i < sz; i++) + if (!hlist_empty(&ht[i])) + return false; + + return true; +} + +/** + * hash_empty - check whether a hashtable is empty + * @hashtable: hashtable to check + * + * This has to be a macro since HASH_BITS() will not work on pointers since + * it calculates the size during preprocessing. + */ +#define hash_empty(hashtable) __hash_empty(hashtable, HASH_SIZE(hashtable)) + +/** + * hash_del - remove an object from a hashtable + * @node: &struct hlist_node of the object to remove + */ +static inline void hash_del(struct hlist_node *node) +{ + hlist_del_init(node); +} + +/** + * hash_del_rcu - remove an object from a rcu enabled hashtable + * @node: &struct hlist_node of the object to remove + */ +static inline void hash_del_rcu(struct hlist_node *node) +{ + hlist_del_init_rcu(node); +} + +/** + * hash_for_each - iterate over a hashtable + * @name: hashtable to iterate + * @bkt: integer to use as bucket loop cursor + * @obj: the type * to use as a loop cursor for each entry + * @member: the name of the hlist_node within the struct + */ +#define hash_for_each(name, bkt, obj, member) \ + for ((bkt) = 0, obj = NULL; obj == NULL && (bkt) < HASH_SIZE(name);\ + (bkt)++)\ + hlist_for_each_entry(obj, &name[bkt], member) + +/** + * hash_for_each_rcu - iterate over a rcu enabled hashtable + * @name: hashtable to iterate + * @bkt: integer to use as bucket loop cursor + * @obj: the type * to use as a loop cursor for each entry + * @member: the name of the hlist_node within the struct + */ +#define hash_for_each_rcu(name, bkt, obj, member) \ + for ((bkt) = 0, obj = NULL; obj == NULL && (bkt) < HASH_SIZE(name);\ + (bkt)++)\ + hlist_for_each_entry_rcu(obj, &name[bkt], member) + +/** + * hash_for_each_safe - iterate over a hashtable safe against removal of + * hash entry + * @name: hashtable to iterate + * @bkt: integer to use as bucket loop cursor + * @tmp: a &struct used for temporary storage + * @obj: the type * to use as a loop cursor for each entry + * @member: the name of the hlist_node within the struct + */ +#define hash_for_each_safe(name, bkt, tmp, obj, member) \ + for ((bkt) = 0, obj = NULL; obj == NULL && (bkt) < HASH_SIZE(name);\ + (bkt)++)\ + hlist_for_each_entry_safe(obj, tmp, &name[bkt], member) + +/** + * hash_for_each_possible - iterate over all possible objects hashing to the + * same bucket + * @name: hashtable to iterate + * @tpos: the type * to use as a loop cursor. + * @pos: the &struct hlist_node to use as a loop cursor. + * @member: the name of the hlist_node within the struct + * @key: the key of the objects to iterate over + */ +#define hash_for_each_possible(name, tpos, pos, member, key) \ + hlist_for_each_entry(tpos, pos, &name[hash_min(key, HASH_BITS(name))], member) + + +/** + * hash_for_each_possible_rcu - iterate over all possible objects hashing to the + * same bucket in an rcu enabled hashtable + * in a rcu enabled hashtable + * @name: hashtable to iterate + * @obj: the type * to use as a loop cursor for each entry + * @member: the name of the hlist_node within the struct + * @key: the key of the objects to iterate over + */ +#define hash_for_each_possible_rcu(name, obj, member, key) \ + hlist_for_each_entry_rcu(obj, &name[hash_min(key, HASH_BITS(name))],\ + member) + +/** + * hash_for_each_possible_rcu_notrace - iterate over all possible objects hashing + * to the same bucket in an rcu enabled hashtable in a rcu enabled hashtable + * @name: hashtable to iterate + * @obj: the type * to use as a loop cursor for each entry + * @member: the name of the hlist_node within the struct + * @key: the key of the objects to iterate over + * + * This is the same as hash_for_each_possible_rcu() except that it does + * not do any RCU debugging or tracing. + */ +#define hash_for_each_possible_rcu_notrace(name, obj, member, key) \ + hlist_for_each_entry_rcu_notrace(obj, \ + &name[hash_min(key, HASH_BITS(name))], member) + +/** + * hash_for_each_possible_safe - iterate over all possible objects hashing to the + * same bucket safe against removals + * @name: hashtable to iterate + * @obj: the type * to use as a loop cursor for each entry + * @tmp: a &struct used for temporary storage + * @member: the name of the hlist_node within the struct + * @key: the key of the objects to iterate over + */ +#define hash_for_each_possible_safe(name, obj, tmp, member, key) \ + hlist_for_each_entry_safe(obj, tmp,\ + &name[hash_min(key, HASH_BITS(name))], member) + + +#endif + + + + + + + struct gtpuhdr { char flags; @@ -59,6 +273,28 @@ struct gtpuhdr #define GTPURH_2_PRINT_BUFFER_LEN 8192 static char _gtpurh_print_buffer[GTPURH_2_PRINT_BUFFER_LEN]; + + +typedef struct ip_fragment_entry_s { + struct sk_buff *skb; + //struct timeval first_ts; + int flags; //ip header field + int offset; //ip header field + int end; + struct hlist_node node; +}ip_fragment_entry_t; + +typedef struct ip_reassembly_entry_s { +}ip_reassembly_entry_t; + + + +#define MAX_IP_FRAGMENTS 16; + +#define GTPURH_HASHTABLE_BITS 7 + +DEFINE_HASHTABLE(ip_fragments, GTPURH_HASHTABLE_BITS); + //----------------------------------------------------------------------------- void _gtpurh_print_hex_octets(unsigned char* dataP, unsigned short sizeP) //----------------------------------------------------------------------------- @@ -78,7 +314,7 @@ void _gtpurh_print_hex_octets(unsigned char* dataP, unsigned short sizeP) h = (tv.tv_sec/3600) % 24; m = (tv.tv_sec / 60) % 60; s = tv.tv_sec % 60; - snprintf(timeofday, 64, "%02d:%02d:%02d.%06d", h,m,s,tv.tv_usec); + snprintf(timeofday, 64, "%02d:%02d:%02d.%06ld", h,m,s,tv.tv_usec); buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker,"%s------+-------------------------------------------------+\n",timeofday); buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker,"%s | 0 1 2 3 4 5 6 7 8 9 a b c d e f |\n",timeofday); @@ -110,7 +346,6 @@ void _gtpurh_print_hex_octets(unsigned char* dataP, unsigned short sizeP) } -#if defined(ROUTE_PACKET) static bool _gtpurh_route_packet(struct sk_buff *skb, const struct xt_gtpurh_target_info *info) { int err = 0; @@ -182,7 +417,114 @@ static bool _gtpurh_route_packet(struct sk_buff *skb, const struct xt_gtpurh_tar return GTPURH_FAILURE; } } + + +static void +_gtpurh_target_reassembly(struct sk_buff *orig_skb, const struct xt_gtpurh_target_info *tgi) +{ + struct iphdr *iph = ip_hdr(orig_skb); + struct sk_buff *skb = NULL; + struct sk_buff *new_skb = NULL; + uint16_t key = 0; + int flags, offset, last_frag_offset; + int ihl, end; + struct ip_fragment_entry_s *ip_fragment_p = NULL; + struct hlist_node *pos_p = NULL; + int all_segments_availables = 1; + + offset = ntohs(iph->frag_off); + flags = offset & ~IP_OFFSET; + offset &= IP_OFFSET; /* offset is in 8-byte chunks */ + offset <<= 3; + ihl = ip_hdrlen(skb); + ihl = ip_hdrlen(skb); + end = offset + skb->len - ihl; + + key = iph->id; + + /* Is this the final fragment? */ + if ((flags & IP_MF) == 0) { + pr_info("GTPURH: try reassembly last segment id %u len %u offset %u end %u\n", + key,orig_skb->len, offset, end); + // may be the last segment received + // normally iterator gives elements from head to tail, but elements + // have been inserted at the head, so iterator should retrieve fragments + // in reverse order. + last_frag_offset = offset; + hash_for_each_possible(ip_fragments, ip_fragment_p, pos_p, node, key) { + pr_info("GTPURH: try reassembly segment id %u len %u offset %u end %u\n", + key,ip_fragment_p->skb->len, ip_fragment_p->offset, ip_fragment_p->end); + if (ip_fragment_p->end == offset) { + offset = ip_fragment_p->offset; + } else { + pr_info("GTPURH: try reassembly segment id %u failed\n", + key); + all_segments_availables = 0; + break; + } + } + if (all_segments_availables) { + // not optimal, but optimize later + new_skb = skb_copy_expand(orig_skb, + last_frag_offset + skb_headroom(orig_skb), + skb_tailroom(orig_skb), + GFP_ATOMIC); + if (new_skb != NULL) + { + hash_for_each_possible(ip_fragments, ip_fragment_p, pos_p, node, key) { + skb = ip_fragment_p->skb; + skb_pull(new_skb, ihl); + + pr_info("GTPURH: reassembly segment id %u len %u offset %u end %u\n", + key, skb->len, ip_fragment_p->offset, ip_fragment_p->end); + + iph = (struct iphdr*)skb_push(new_skb, skb->len); + ihl = ip_hdrlen(skb); + memcpy(iph, skb_network_header(skb), skb->len); + } + skb_set_network_header(new_skb, 0); + if (_gtpurh_route_packet(new_skb, tgi) != GTPURH_SUCCESS) + { + kfree_skb(new_skb); + } + + } else { + pr_info("GTPURH: Failed in skb allocation (%u bytes)\n", + last_frag_offset + skb_headroom(orig_skb)); + } + } + if (1) { + hash_for_each_possible(ip_fragments, ip_fragment_p, pos_p, node, key) { + kfree_skb(ip_fragment_p->skb); + hash_del(pos_p); + kfree(ip_fragment_p); + } + } + } else { + pr_info("GTPURH: but in buffer not last segment id %u len %u offset %u end %u\n", + key,orig_skb->len, offset, end); + // not the last fragment received + // assuming no out of order + ip_fragment_p = kmalloc(sizeof(struct ip_fragment_entry_s), GFP_KERNEL); + + if (NULL != ip_fragment_p) { +#if defined(ROUTE_PACKET) + ip_fragment_p->skb = orig_skb; +#else + ip_fragment_p->skb = skb_copy(orig_skb, GFP_ATOMIC); // may use skb_clone(), TODO #endif + ip_fragment_p->flags = flags; + ip_fragment_p->offset = offset; + ip_fragment_p->end = end; + + INIT_HLIST_NODE(&ip_fragment_p->node); + hash_add(ip_fragments, &ip_fragment_p->node, key); + } else { + pr_info("GTPURH: ERROR could not allocate memory for buffering IP fragment\n"); + } + } +} + static unsigned int _gtpurh_target_rem(struct sk_buff *orig_skb, const struct xt_gtpurh_target_info *tgi) @@ -207,6 +549,11 @@ _gtpurh_target_rem(struct sk_buff *orig_skb, const struct xt_gtpurh_target_info skb = orig_skb; #endif + if (ip_is_fragment(iph)) { + _gtpurh_target_reassembly(skb, tgi); + return NF_DROP; // orig_skb is copied, original can be destroyed. + } + /* Remove IP header */ udph = (struct udphdr*)skb_pull(skb, (iph->ihl << 2)); @@ -226,7 +573,7 @@ _gtpurh_target_rem(struct sk_buff *orig_skb, const struct xt_gtpurh_target_info skb->mark, ntohs(iph->frag_off) & 0x1FFFFFFF, ntohs(iph->frag_off) >> 13); - return NF_DROP; + return NF_ACCEPT; } /* Remove UDP header */ @@ -249,6 +596,11 @@ _gtpurh_target_rem(struct sk_buff *orig_skb, const struct xt_gtpurh_target_info iph2 = ip_hdr(skb); skb_set_transport_header(skb, iph2->ihl << 2); + if ((iph2->version != 4 ) && (iph2->version != 6)) { + pr_info("\nGTPURH: Decapsulated packet dropped because not IPvx protocol see all GTPU packet here:\n"); + _gtpurh_print_hex_octets((unsigned char*)iph, ntohs(iph->tot_len)); + return NF_DROP; + } //#if 0 if ((skb->mark == 0) || (gtp_payload_size != ntohs(iph2->tot_len))) { pr_info("\nGTPURH(%d): Decapsulated packet: %d.%d.%d.%d --> %d.%d.%d.%d Proto: %d, Total Len (IP): %u mark %u Frag offset %u Flags 0x%0x\n", @@ -272,7 +624,7 @@ _gtpurh_target_rem(struct sk_buff *orig_skb, const struct xt_gtpurh_target_info tgi->action, ntohs(gtpuh->length), gtp_payload_size, - ntohs(iph->tot_len)); + ntohs(iph2->tot_len)); _gtpurh_print_hex_octets((unsigned char*)iph, ntohs(iph->tot_len)); } @@ -323,6 +675,7 @@ static int __init xt_gtpurh_init(void) { pr_info("GTPURH: Initializing module (KVersion: %d)\n", KVERSION); pr_info("GTPURH: Copyright Polaris Networks 2010-2011\n"); + hash_init(ip_fragments); return xt_register_target(&xt_gtpurh_reg); } diff --git a/openair-cn/GTPV1-U/GTPURH/xt_GTPURH.h b/openair-cn/GTPV1-U/GTPURH/xt_GTPURH.h index 0044ca73cc..6ee3c087cf 100755 --- a/openair-cn/GTPV1-U/GTPURH/xt_GTPURH.h +++ b/openair-cn/GTPV1-U/GTPURH/xt_GTPURH.h @@ -23,3 +23,4 @@ struct xt_gtpurh_target_info u_int32_t action; }; +