Tested ok for downlink segmentation du to GTPU encapsulation

git-svn-id: http://svn.eurecom.fr/openair4G/trunk@6074 818b1a75-f10b-46b9-bf7c-635c3b92a50f

openair-cn/GTPV1-U/GTPUAH/xt_GTPUAH.c

@@ -17,6 +17,7 @@
 #include <linux/skbuff.h>
 #include <linux/ip.h>
 #include <linux/route.h> 
+#include <linux/time.h>
 #include <net/checksum.h>
 #include <net/udp.h>
 #include <net/inet_sock.h>
@@ -47,6 +48,7 @@ struct gtpuhdr
     u_int32_t tunid;
 };
 
+#define MTU            1500
 #define GTPU_HDR_PNBIT 1
 #define GTPU_HDR_SBIT 1 << 1
 #define GTPU_HDR_EBIT 1 << 2
@@ -57,6 +59,9 @@ struct gtpuhdr
 
 #define GTPU_PORT 2152
 
+#define IP_MORE_FRAGMENTS 0x2000
+
+
 static bool _gtpuah_route_packet(struct sk_buff *skb, const struct xt_gtpuah_target_info *info)
 {
     int err = 0; 
@@ -133,25 +138,109 @@ static unsigned int
 _gtpuah_target_add(struct sk_buff *skb, const struct xt_gtpuah_target_info *tgi)
 {
     struct iphdr   *iph           = ip_hdr(skb);
+    struct iphdr   *iph2          = NULL;
     struct udphdr  *udph          = NULL;
     struct gtpuhdr *gtpuh         = NULL;
     struct sk_buff *new_skb       = NULL;
-    int             headroom_reqd =  sizeof(struct iphdr) + sizeof(struct udphdr) + sizeof(struct gtpuhdr);
-    int             orig_iplen = 0, udp_len = 0, ip_len = 0;
+    struct sk_buff *new_skb2      = NULL;
+    uint16_t        headroom_reqd =  sizeof(struct iphdr) + sizeof(struct udphdr) + sizeof(struct gtpuhdr);
+    uint16_t        orig_iplen = 0, udp_len = 0, ip_len = 0;
 
     if (skb->mark == 0) {
-        pr_info("GTPUAH: _gtpuah_target_add skb mark %u tgi mark %u", skb->mark, tgi->rtun);
+        pr_info("GTPUAH: _gtpuah_target_add force skb mark %u to tgi mark %u", skb->mark, tgi->rtun);
+        skb->mark = tgi->rtun;
     }
     /* Keep the length of the source IP packet */
     orig_iplen = ntohs(iph->tot_len);
 
+
     /* Create a new copy of the original skb...can't avoid :-( */
+    // LG: have a look at pskb_expand_head
     new_skb = skb_copy_expand(skb, headroom_reqd + skb_headroom(skb), skb_tailroom(skb), GFP_ATOMIC);
     if (new_skb == NULL)
     {
         return NF_ACCEPT;
     }
 
+    // must segment if added headers (IP, UDP, GTP) + original data + original headers > MTU
+    if ((orig_iplen + headroom_reqd) > MTU) {
+        pr_info("GTPUAH: Fragmentation Id %04X size %u, %u",
+                (uint16_t)new_skb,
+                MTU,
+                sizeof(struct iphdr) + orig_iplen - MTU + headroom_reqd);
+
+        skb_trim(new_skb, MTU - headroom_reqd);
+
+        gtpuh          = (struct gtpuhdr*)skb_push(new_skb, sizeof(struct gtpuhdr));
+        gtpuh->flags   = 0x30; /* v1 and Protocol-type=GTP */
+        gtpuh->msgtype = 0xff; /* T-PDU */
+        gtpuh->length  = htons(MTU - headroom_reqd);
+        gtpuh->tunid   = htonl(skb->mark);
+
+        /* Add UDP header */
+        udp_len      = sizeof(struct udphdr) + sizeof(struct gtpuhdr) + orig_iplen;
+        udph         = (struct udphdr*)skb_push(new_skb, sizeof(struct udphdr));
+        udph->source = htons(GTPU_PORT);
+        udph->dest   = htons(GTPU_PORT);
+        udph->len    = htons(udp_len);
+        udph->check  = 0;
+        udph->check  = csum_tcpudp_magic(tgi->laddr,
+                tgi->raddr,
+                udp_len,
+                IPPROTO_UDP,
+                csum_partial((char*)udph, udp_len, 0));
+        skb_set_transport_header(new_skb, 0);
+
+        /* Add IP header */
+        ip_len = MTU;
+        iph = (struct iphdr*)skb_push(new_skb, sizeof(struct iphdr));
+        iph->ihl      = 5;
+        iph->version  = 4;
+        iph->tos      = 0;
+        iph->tot_len  = htons(ip_len);
+        iph->id       = htons((uint16_t)new_skb);
+        iph->frag_off = htons(IP_MORE_FRAGMENTS);
+        iph->ttl      = 64;
+        iph->protocol = IPPROTO_UDP;
+        iph->saddr    = (tgi->laddr);
+        iph->daddr    = (tgi->raddr);
+        iph->check    = 0;
+        iph->check    = ip_fast_csum((unsigned char *)iph, iph->ihl);
+        skb_set_network_header(new_skb, 0);
+
+        new_skb2 = skb_copy(skb, GFP_ATOMIC);
+        skb_pull(new_skb2, MTU - headroom_reqd);
+        /* Add IP header */
+        iph2 = (struct iphdr*)skb_push(new_skb2, sizeof(struct iphdr));
+        iph2->ihl      = 5;
+        iph2->version  = 4;
+        iph2->tos      = 0;
+        iph2->tot_len  = htons(sizeof(struct iphdr) + orig_iplen - MTU + headroom_reqd);
+        iph2->id       = htons((uint16_t)new_skb);
+        iph2->frag_off = htons((MTU - sizeof(struct iphdr)) / 8);
+        iph2->ttl      = 64;
+        iph2->protocol = IPPROTO_UDP;
+        iph2->saddr    = (tgi->laddr);
+        iph2->daddr    = (tgi->raddr);
+        iph2->check    = 0;
+        iph2->check    = ip_fast_csum((unsigned char *)iph2, iph2->ihl);
+        skb_set_network_header(new_skb2, 0);
+
+        /* Route the packet */
+        if (_gtpuah_route_packet(new_skb, tgi) == GTPU_SUCCESS)
+        {
+            if (_gtpuah_route_packet(new_skb2, tgi) != GTPU_SUCCESS) {
+                kfree_skb(new_skb2);
+            }
+            /* Succeeded. Drop the original packet */
+            return NF_DROP;
+        }
+        else
+        {
+            kfree_skb(new_skb);
+            return NF_ACCEPT; /* What should we do here ??? ACCEPT seems to be the best option */
+        }
+    } else {
         /* Add GTPu header */
         gtpuh          = (struct gtpuhdr*)skb_push(new_skb, sizeof(struct gtpuhdr));
         gtpuh->flags   = 0x30; /* v1 and Protocol-type=GTP */
@@ -174,7 +263,7 @@ _gtpuah_target_add(struct sk_buff *skb, const struct xt_gtpuah_target_info *tgi)
         skb_set_transport_header(new_skb, 0);
 
         /* Add IP header */
-    ip_len = udp_len + sizeof(struct iphdr);
+        ip_len = sizeof(struct iphdr) + sizeof(struct udphdr) + sizeof(struct gtpuhdr) + orig_iplen;
         iph = (struct iphdr*)skb_push(new_skb, sizeof(struct iphdr));
         iph->ihl      = 5;
         iph->version  = 4;
@@ -184,9 +273,9 @@ _gtpuah_target_add(struct sk_buff *skb, const struct xt_gtpuah_target_info *tgi)
         iph->frag_off = 0;
         iph->ttl      = 64;
         iph->protocol = IPPROTO_UDP;
-    iph->check    = 0;
         iph->saddr    = (tgi->laddr);
         iph->daddr    = (tgi->raddr);
+        iph->check    = 0;
         iph->check    = ip_fast_csum((unsigned char *)iph, iph->ihl);
         skb_set_network_header(new_skb, 0);
 
@@ -201,6 +290,7 @@ _gtpuah_target_add(struct sk_buff *skb, const struct xt_gtpuah_target_info *tgi)
             kfree_skb(new_skb);
             return NF_ACCEPT; /* What should we do here ??? ACCEPT seems to be the best option */
         }
+    }
 }
 
 

openair-cn/GTPV1-U/GTPURH/xt_GTPURH.c

@@ -57,6 +57,60 @@ struct gtpuhdr
 
 #define GTPURH_PORT 2152
 
+#define GTPURH_2_PRINT_BUFFER_LEN 8192
+static char _gtpurh_print_buffer[GTPURH_2_PRINT_BUFFER_LEN];
+//-----------------------------------------------------------------------------
+void _gtpurh_print_hex_octets(unsigned char* dataP, unsigned short sizeP)
+//-----------------------------------------------------------------------------
+{
+  unsigned long octet_index = 0;
+  unsigned long buffer_marker = 0;
+  unsigned char aindex;
+  struct timeval tv;
+  char timeofday[64];
+  unsigned int h,m,s;
+
+  if (dataP == NULL) {
+    return;
+  }
+
+  do_gettimeofday(&tv);
+  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);
+
+  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);
+  buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker,"%s------+-------------------------------------------------+\n",timeofday);
+  for (octet_index = 0; octet_index < sizeP; octet_index++) {
+    if ((octet_index % 16) == 0){
+      if (octet_index != 0) {
+          buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker, " |\n");
+      }
+      buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker, "%s %04ld |",timeofday, octet_index);
+    }
+    /*
+     * Print every single octet in hexadecimal form
+     */
+    buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker, " %02x", dataP[octet_index]);
+    /*
+     * Align newline and pipes according to the octets in groups of 2
+     */
+  }
+
+  /*
+   * Append enough spaces and put final pipe
+   */
+  for (aindex = octet_index; aindex < 16; ++aindex)
+    buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker, "   ");
+    //SGI_IF_DEBUG("   ");
+  buffer_marker+=snprintf(&_gtpurh_print_buffer[buffer_marker], GTPURH_2_PRINT_BUFFER_LEN - buffer_marker, " |\n");
+  pr_info("%s",_gtpurh_print_buffer);
+}
+
+
+#if defined(ROUTE_PACKET)
 static bool _gtpurh_route_packet(struct sk_buff *skb, const struct xt_gtpurh_target_info *info)
 {
     int err = 0; 
@@ -128,12 +182,14 @@ static bool _gtpurh_route_packet(struct sk_buff *skb, const struct xt_gtpurh_tar
         return GTPURH_FAILURE;
     }
 }
+#endif
 
 static unsigned int
 _gtpurh_target_rem(struct sk_buff *orig_skb, const struct xt_gtpurh_target_info *tgi)
 {
     struct iphdr   *iph   = ip_hdr(orig_skb);
     struct iphdr   *iph2  = NULL;
+    struct udphdr  *udph  = NULL;
     struct gtpuhdr *gtpuh = NULL;
     struct sk_buff *skb   = NULL;
     uint16_t        gtp_payload_size = 0;
@@ -152,7 +208,26 @@ _gtpurh_target_rem(struct sk_buff *orig_skb, const struct xt_gtpurh_target_info
 #endif
 
     /* Remove IP header */
-    skb_pull(skb, (iph->ihl << 2));
+    udph = (struct udphdr*)skb_pull(skb, (iph->ihl << 2));
+
+    if (iph->protocol != IPPROTO_UDP) {
+        pr_info("GTPURH(%d): ERROR in decapsulating packet: %d.%d.%d.%d --> %d.%d.%d.%d Bad Proto: %d, Total Len (IP): %u mark %u Frag offset %u Flags 0x%0x\n",
+                tgi->action,
+                iph->saddr  & 0xFF,
+                (iph->saddr & 0x0000FF00) >> 8,
+                (iph->saddr & 0x00FF0000) >> 16,
+                iph->saddr >> 24,
+                iph->daddr  & 0xFF,
+                (iph->daddr & 0x0000FF00) >> 8,
+                (iph->daddr & 0x00FF0000) >> 16,
+                iph->daddr >> 24,
+                iph->protocol,
+                ntohs(iph2->tot_len),
+                skb->mark,
+                ntohs(iph->frag_off) & 0x1FFFFFFF,
+                ntohs(iph->frag_off) >> 13);
+        return NF_DROP;
+    }
 
     /* Remove UDP header */
     gtpuh = (struct gtpuhdr*)skb_pull(skb, sizeof(struct udphdr));
@@ -176,7 +251,7 @@ _gtpurh_target_rem(struct sk_buff *orig_skb, const struct xt_gtpurh_target_info
 
 //#if 0
     if ((skb->mark == 0) || (gtp_payload_size != ntohs(iph2->tot_len))) {
-        pr_info("GTPURH(%d): Decapsulating packet: %d.%d.%d.%d --> %d.%d.%d.%d Proto: %d, Total Len (IP): %u mark %u Frag offset %u Flags 0x%0x\n",
+        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",
                 tgi->action,
                 iph2->saddr  & 0xFF,
                 (iph2->saddr & 0x0000FF00) >> 8,
@@ -193,8 +268,13 @@ _gtpurh_target_rem(struct sk_buff *orig_skb, const struct xt_gtpurh_target_info
                 ntohs(iph->frag_off) >> 13);
 
         if (gtp_payload_size != ntohs(iph2->tot_len)) {
-            pr_info("\nGTPURH(%d): Mismatch in lengths GTPU length: %u -> %u, IP length %u\n\n",
-                    ntohs(gtpuh->length), gtp_payload_size, ntohs(iph->tot_len));
+            pr_info("GTPURH(%d): Mismatch in lengths GTPU length: %u -> %u, IP length %u\n",
+                    tgi->action,
+                    ntohs(gtpuh->length),
+                    gtp_payload_size,
+                    ntohs(iph->tot_len));
+
+            _gtpurh_print_hex_octets((unsigned char*)iph, ntohs(iph->tot_len));
         }
     }
 //#endif