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

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

@@ -78,9 +78,17 @@ static bool _gtpuah_route_packet(struct sk_buff *skb, const struct xt_gtpuah_tar
         } 
     }; 
 
-#if 0
+#if 1
-    pr_info("GTPUAH(%d): Routing packet: %d.%d.%d.%d --> %d.%d.%d.%d Proto: %d, Len: %d Mark: %u\n",
+    int                         flags, offset;
+
+    offset = ntohs(iph->frag_off);
+    flags  = offset & ~IP_OFFSET;
+    offset &= IP_OFFSET; /* offset is in 8-byte chunks */
+    offset <<= 3;
+
+    pr_info("GTPUAH(%d): Routing %s packet: %d.%d.%d.%d --> %d.%d.%d.%d Proto: %d, Len: %05u, Id %04X, Offset %u, Flags %04X, Mark: %u\n",
             info->action,
+            (ip_is_fragment(iph) == 0) ? "":"fragmented",
             iph->saddr  & 0xFF,
             (iph->saddr & 0x0000FF00) >> 8,
             (iph->saddr & 0x00FF0000) >> 16,
@@ -91,6 +99,9 @@ static bool _gtpuah_route_packet(struct sk_buff *skb, const struct xt_gtpuah_tar
             iph->daddr >> 24,
             iph->protocol,
             ntohs(iph->tot_len),
+            ntohs(iph->id),
+            offset,
+            flags,
             skb->mark);
 #endif
 
@@ -145,6 +156,9 @@ _gtpuah_target_add(struct sk_buff *skb, const struct xt_gtpuah_target_info *tgi)
     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;
+    int16_t         remaining_bytes_to_send = 0, remaining_payload_to_send = 0,
+            payload_to_send = 0, bytes_to_remove = 0, bytes_to_send = 0;
+    int             flags = 0, offset = 0, offset2 = 0;
 
     if (skb->mark == 0) {
         pr_info("GTPUAH: _gtpuah_target_add force skb mark %u to tgi mark %u", skb->mark, tgi->rtun);
@@ -152,95 +166,22 @@ _gtpuah_target_add(struct sk_buff *skb, const struct xt_gtpuah_target_info *tgi)
     }
     /* Keep the length of the source IP packet */
     orig_iplen = ntohs(iph->tot_len);
+    offset = ntohs(iph->frag_off);
+    flags  = offset & ~IP_OFFSET;
 
 
     /* Create a new copy of the original skb...can't avoid :-( */
-    // LG: have a look at pskb_expand_head
+    if (((orig_iplen + headroom_reqd) <= MTU) || (flags & IP_DF)) {
-    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 (flags & IP_DF) {
-    if ((orig_iplen + headroom_reqd) > MTU) {
+            pr_info("GTPUAH: DONT FRAGMENT id %04X", ntohs(iph->id));
-        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
+
+        new_skb = skb_copy_expand(skb, headroom_reqd + skb_headroom(skb), skb_tailroom(skb), GFP_ATOMIC);
+        if (new_skb == NULL)
         {
-            kfree_skb(new_skb);
+            return NF_ACCEPT;
-            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 */
@@ -269,7 +210,7 @@ _gtpuah_target_add(struct sk_buff *skb, const struct xt_gtpuah_target_info *tgi)
         iph->version  = 4;
         iph->tos      = 0;
         iph->tot_len  = htons(ip_len);
-        iph->id       = 0;
+        iph->id       = (uint16_t)(((uint64_t)new_skb) >> 8);
         iph->frag_off = 0;
         iph->ttl      = 64;
         iph->protocol = IPPROTO_UDP;
@@ -290,6 +231,114 @@ _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 */
         }
+    } else {
+        new_skb = skb_copy_expand(skb, headroom_reqd + skb_headroom(skb), 0, GFP_ATOMIC);
+        if (new_skb == NULL)
+        {
+            return NF_ACCEPT;
+        }
+
+        // must segment if added headers (IP, UDP, GTP) + original data + original headers > MTU
+        pr_info("GTPUAH: Fragmentation Id %04X payload size %u (IP Head inc), total ip packet size %u",
+                htons((uint16_t)(((uint64_t)new_skb) >> 8)),
+                MTU - headroom_reqd,
+                MTU);
+
+
+        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(orig_iplen);
+        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       = (uint16_t)(((uint64_t)new_skb) >> 8);
+        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);
+
+        skb_trim(new_skb, MTU - headroom_reqd);
+
+        /* Route the packet */
+        if (_gtpuah_route_packet(new_skb, tgi) == GTPU_SUCCESS)
+        {
+            remaining_payload_to_send = orig_iplen - MTU + headroom_reqd;
+            remaining_bytes_to_send   = remaining_payload_to_send + sizeof(struct iphdr);
+            bytes_to_remove = MTU - headroom_reqd;
+            offset2 = MTU - headroom_reqd - sizeof(struct iphdr);
+
+            // the localhost generated traffic can have len > MTU
+            // but can cause problem at UE rx side
+            while (remaining_payload_to_send > 0) {
+                if ((remaining_payload_to_send + sizeof(struct iphdr)) > MTU) {
+                    bytes_to_send = MTU;
+                    payload_to_send = MTU - sizeof(struct iphdr);
+                } else {
+                    bytes_to_send = remaining_payload_to_send + sizeof(struct iphdr);
+                    payload_to_send = remaining_payload_to_send;
+                }
+                pr_info("GTPUAH: Fragmentation Id %04X payload size %u, total ip packet size %u",
+                        htons((uint16_t)(((uint64_t)new_skb) >> 8)),
+                        payload_to_send,
+                        bytes_to_send);
+                // Not optimal, TO OPTIMIZE copies
+                new_skb2 = skb_copy(skb, GFP_ATOMIC);
+                skb_pull(new_skb2, bytes_to_remove);
+                /* 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(bytes_to_send);
+                iph2->id       = (uint16_t)(((uint64_t)new_skb) >> 8);
+                iph2->frag_off = htons(offset2 / 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);
+                skb_trim(new_skb2, MTU);
+
+                _gtpuah_route_packet(new_skb2, tgi);
+
+                remaining_payload_to_send = remaining_payload_to_send - payload_to_send;
+                bytes_to_remove = bytes_to_remove + payload_to_send;
+                offset2 = offset2 + payload_to_send;
+            }
+            return NF_DROP;
+        }
+        else
+        {
+            kfree_skb(new_skb);
+            return NF_ACCEPT; /* What should we do here ??? ACCEPT seems to be the best option */
+        }
     }
 }