diff --git a/openair-cn/GTPV1-U/GTPUAH/xt_GTPUAH.c b/openair-cn/GTPV1-U/GTPUAH/xt_GTPUAH.c
index 4eec85ee6fcab1c907a1da9e2260405b6f459a27..7a27fdead85bd4a7d0d3887ef54f758b4bc1ef41 100755
--- a/openair-cn/GTPV1-U/GTPUAH/xt_GTPUAH.c
+++ b/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,73 +138,158 @@ 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;
}
- /* Add GTPu header */
- 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 = udp_len + sizeof(struct iphdr);
- 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 = 0;
- iph->frag_off = 0;
- iph->ttl = 64;
- iph->protocol = IPPROTO_UDP;
- iph->check = 0;
- iph->saddr = (tgi->laddr);
- iph->daddr = (tgi->raddr);
- iph->check = ip_fast_csum((unsigned char *)iph, iph->ihl);
- skb_set_network_header(new_skb, 0);
-
- /* Route the packet */
- if (_gtpuah_route_packet(new_skb, tgi) == GTPU_SUCCESS)
- {
- /* 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 */
+ // 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 */
+ 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 = 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;
+ iph->tos = 0;
+ iph->tot_len = htons(ip_len);
+ iph->id = 0;
+ iph->frag_off = 0;
+ 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);
+
+ /* Route the packet */
+ if (_gtpuah_route_packet(new_skb, tgi) == GTPU_SUCCESS)
+ {
+ /* 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 */
+ }
}
}
diff --git a/openair-cn/GTPV1-U/GTPURH/xt_GTPURH.c b/openair-cn/GTPV1-U/GTPURH/xt_GTPURH.c
index 01f82c2f22e1f095d63e410866d92c1093fb0cd4..62813635854185ddaa600801dd30533c7864e526 100755
--- a/openair-cn/GTPV1-U/GTPURH/xt_GTPURH.c
+++ b/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