Commit 0f29fdca authored by gauthier's avatar gauthier

Tested ok for downlink segmentation du to GTPU encapsulation

git-svn-id: http://svn.eurecom.fr/openair4G/trunk@6074 818b1a75-f10b-46b9-bf7c-635c3b92a50f
parent ae1c3b05
......@@ -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 */
}
}
}
......
......@@ -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
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment