Skip to content

O
OpenXG-RAN
Commit aeb743eb authored by Lionel Gauthier's avatar Lionel Gauthier
Browse files
Options

socket no wait only for debugging 

git-svn-id: http://svn.eurecom.fr/openair4G/trunk@7427 818b1a75-f10b-46b9-bf7c-635c3b92a50f
parent d1da9620
Hide whitespace changes
Inline Side-by-side
Showing with 772 additions and 768 deletions
openair-cn/GTPV1-U/GTPUAH/xt_GTPUAH.c
View file @ aeb743eb
/*
* GTPu klm for Linux/iptables
*
* Copyright (c) 2010-2011 Polaris Networks
* Author: Pradip Biswas <pradip_biswas@polarisnetworks.net>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
*/
#include <linux/module.h>
#include <linux/kallsyms.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/skbuff.h>
#include <linux/ip.h>
#include <linux/if_ether.h>
#include <linux/route.h>
#include <linux/time.h>
#include <net/checksum.h>
#include <net/ip.h>
#include <linux/in.h>
#include <linux/icmp.h>
#include <net/udp.h>
#include <net/inet_sock.h>
#include <net/route.h>
#include <net/addrconf.h>
#include <net/ip6_checksum.h>
#include <net/ip6_route.h>
#include <net/ipv6.h>
#include <linux/netfilter/x_tables.h>
#include <linux/netfilter_ipv4/ip_tables.h>
#include <linux/kthread.h>
#include <linux/errno.h>
#include <linux/types.h>
#include <linux/netdevice.h>
#include <linux/delay.h>
#include <linux/inet.h>
// CONNMARK
#include <net/netfilter/nf_conntrack.h>
#include <net/netfilter/nf_conntrack_ecache.h>
#include <linux/netfilter/x_tables.h>
#include <linux/netfilter/xt_connmark.h>
#if defined(CONFIG_IP6_NF_IPTABLES) || defined(CONFIG_IP6_NF_IPTABLES_MODULE)
# define WITH_IPV6 1
#endif
#include "xt_GTPUAH.h"
#if !(defined KVERSION)
# error "Kernel version is not defined!!!! Exiting."
#endif
#undef THREAD_SOCK_NO_WAIT
#define TRACE_IN_KERN_LOG 1
#define TRACE_ICMP_IN_KERN_LOG 1
#if defined(TRACE_IN_KERN_LOG)
#define PR_INFO(fORMAT, aRGS...) pr_info(fORMAT, ##aRGS)
#else
#define PR_INFO(fORMAT, aRGS...)
#endif
#define INT_MODULE_PARM(n, v) static int n = v; module_param(n, int, 0444)
#define STRING_MODULE_PARM(s, v) static char* s = v; module_param(s, charp, 0000);
//-----------------------------------------------------------------------------
static char* _gtpuah_nf_inet_hook_2_string(int nf_inet_hookP);
static void _gtpuah_print_hex_octets(unsigned char* data_pP, unsigned short sizeP);
static void _gtpuah_tg4_add(struct sk_buff *old_skb_pP, const struct xt_action_param *par_pP);
#ifdef WITH_IPV6
static void _gtpuah_tg6_add(struct sk_buff *old_skb_pP, const struct xt_action_param *par_pP);
static unsigned int _gtpuah_tg6(struct sk_buff *skb_pP, const struct xt_action_param *par_pP);
#endif
static unsigned int _gtpuah_tg4(struct sk_buff *skb_pP, const struct xt_action_param *par_pP);
static int __init gtpuah_tg_init(void);
static void __exit gtpuah_tg_exit(void);
static int _udp_thread(void *data);
static int _gtpuah_ksocket_send(struct socket *sock_pP, struct sockaddr_in *addr_pP, unsigned char *gtpuh_pP, int len_gtpP, unsigned char *buf_ip_pP, int len_ipP);
static int _gtpuah_ksocket_receive(struct socket* sock_pP, struct sockaddr_in* addr_pP, unsigned char* buf_pP, int lenP);
static int _gtpuah_ksocket_process_gtp(const unsigned char * const rx_buf_pP, const int lenP, unsigned char* tx_buf_pP);
//-----------------------------------------------------------------------------
#define MODULE_NAME "GTPUAH"
MODULE_LICENSE("GPL");
MODULE_AUTHOR("Pradip Biswas <pradip_biswas@polarisnetworks.net>");
MODULE_DESCRIPTION("GTPu Data Path extension on netfilter");
//-----------------------------------------------------------------------------
static struct xt_target gtpuah_tg_reg[] __read_mostly = {
{
.name = MODULE_NAME,
.revision = 0,
.family = NFPROTO_IPV4,
.hooks = (1 << NF_INET_POST_ROUTING) |
(1 << NF_INET_LOCAL_IN) |
(1 << NF_INET_FORWARD),
.table = "mangle",
.target = _gtpuah_tg4,
.targetsize = sizeof(struct xt_gtpuah_target_info),
.me = THIS_MODULE,
},
#ifdef WITH_IPV6
{
.name = MODULE_NAME,
.revision = 0,
.family = NFPROTO_IPV6,
.hooks = (1 << NF_INET_POST_ROUTING) |
(1 << NF_INET_LOCAL_IN) |
(1 << NF_INET_FORWARD),
.table = "mangle",
.target = _gtpuah_tg6,
.targetsize = sizeof(struct xt_gtpuah_target_info),
.me = THIS_MODULE,
},
#endif
};
#define GTP_ECHO_REQ 1
#define GTP_ECHO_RSP 2
#define GTP_ERROR_INDICATION 26
#define GTP_GPDU 255
typedef struct gtpv1u_msg_s {
unsigned char version;
unsigned char protocol_type;
unsigned char ext_hdr_flag;
unsigned char seq_num_flag;
u_int16_t npdu_num_flag;
u_int32_t msg_type;
u_int16_t msg_len;
u_int32_t teid;
u_int16_t seq_num;
unsigned char npdu_num;
unsigned char next_ext_hdr_type;
u_int32_t msg_buf_len;
u_int32_t msg_buf_offset;
struct gtpv1u_msg_s* next;
} gtpv1u_msg_t;
struct gtpuhdr {
char flags;
char msgtype;
u_int16_t length;
u_int32_t tunid;
};
typedef struct gtpuah_sock_s {
struct task_struct *thread;
struct sockaddr_in addr;
struct socket *sock;
struct sockaddr_in addr_send;
int running;
int thread_stop_requested;
} gtpuah_sock_t;
//-----------------------------------------------------------------------------
#define GTPU_HDR_PNBIT 1
#define GTPU_HDR_SBIT 1 << 1
#define GTPU_HDR_EBIT 1 << 2
#define GTPU_ANY_EXT_HDR_BIT (GTPU_HDR_PNBIT | GTPU_HDR_SBIT | GTPU_HDR_EBIT)
#define GTPU_FAILURE 1
#define GTPU_SUCCESS !GTPU_FAILURE
#define GTPUAH_2_PRINT_BUFFER_LEN 8192
#define IP_MORE_FRAGMENTS 0x2000
#define NIPADDR(addr) \
(uint8_t)(addr & 0x000000FF), \
(uint8_t)((addr & 0x0000FF00) >> 8), \
(uint8_t)((addr & 0x00FF0000) >> 16), \
(uint8_t)((addr & 0xFF000000) >> 24)
//-----------------------------------------------------------------------------
static char _gtpuah_print_buffer[GTPUAH_2_PRINT_BUFFER_LEN];
gtpuah_sock_t _gtpuah_sock;
INT_MODULE_PARM(gtpu_sgw_port, 2152);
MODULE_PARM_DESC(gtpu_sgw_port, "UDP port number for S1U interface (s-GW side)");
INT_MODULE_PARM(gtpu_enb_port, 2153);
MODULE_PARM_DESC(gtpu_enb_port, "UDP port number for S1U interface (eNB side)");
STRING_MODULE_PARM(sgw_addr, "127.0.0.1");
MODULE_PARM_DESC(sgw_addr, "IPv4 address of the S1U IP interface");
//-----------------------------------------------------------------------------
static char*
_gtpuah_icmph_type_2_string(uint8_t typeP)
//-----------------------------------------------------------------------------
{
switch (typeP) {
case ICMP_ECHOREPLY:return "ECHOREPLY";break;
case ICMP_DEST_UNREACH:return "DEST_UNREACH";break;
case ICMP_SOURCE_QUENCH:return "SOURCE_QUENCH";break;
case ICMP_REDIRECT:return "REDIRECT";break;
case ICMP_ECHO:return "ECHO";break;
case ICMP_TIME_EXCEEDED:return "TIME_EXCEEDED";break;
case ICMP_PARAMETERPROB:return "PARAMETERPROB";break;
case ICMP_TIMESTAMP:return "TIMESTAMP";break;
case ICMP_TIMESTAMPREPLY:return "TIMESTAMPREPLY";break;
case ICMP_INFO_REQUEST:return "INFO_REQUEST";break;
case ICMP_INFO_REPLY:return "INFO_REPLY";break;
case ICMP_ADDRESS:return "ADDRESS";break;
case ICMP_ADDRESSREPLY:return "ADDRESSREPLY";break;
default:return "TYPE?";
}
}
//-----------------------------------------------------------------------------
static char*
_gtpuah_nf_inet_hook_2_string(int nf_inet_hookP)
{
//-----------------------------------------------------------------------------
switch (nf_inet_hookP) {
case NF_INET_PRE_ROUTING:
return "NF_INET_PRE_ROUTING";
break;
case NF_INET_LOCAL_IN:
return "NF_INET_LOCAL_IN";
break;
case NF_INET_FORWARD:
return "NF_INET_FORWARD";
break;
case NF_INET_LOCAL_OUT:
return "NF_INET_LOCAL_OUT";
break;
case NF_INET_POST_ROUTING:
return "NF_INET_POST_ROUTING";
break;
default:
return "NF_INET_UNKNOWN";
}
}
//-----------------------------------------------------------------------------
void
_gtpuah_print_hex_octets(unsigned char* data_pP, 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 (data_pP == NULL) {
return;
}
if (sizeP > 2000) {
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.%06ld", h,m,s,tv.tv_usec);
buffer_marker+=snprintf(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker,"%s------+-------------------------------------------------+\n",timeofday);
buffer_marker+=snprintf(&_gtpuah_print_buffer[buffer_marker], GTPUAH_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(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker,"%s------+-------------------------------------------------+\n",timeofday);
pr_info("%s",_gtpuah_print_buffer);
buffer_marker = 0;
for (octet_index = 0; octet_index < sizeP; octet_index++) {
if ((octet_index % 16) == 0) {
if (octet_index != 0) {
buffer_marker+=snprintf(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker, " |\n");
pr_info("%s",_gtpuah_print_buffer);
buffer_marker = 0;
}
buffer_marker+=snprintf(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker, "%s %04ld |",timeofday, octet_index);
}
/*
* Print every single octet in hexadecimal form
*/
buffer_marker+=snprintf(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker, " %02x", data_pP[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(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker, " ");
//SGI_IF_DEBUG(" ");
buffer_marker+=snprintf(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker, " |\n");
pr_info("%s",_gtpuah_print_buffer);
}
//-----------------------------------------------------------------------------
static int _udp_thread(void *data)
//-----------------------------------------------------------------------------
{
int size, tx_size;
int bufsize = 8192;
unsigned char buf[bufsize+1];
unsigned char gtp_resp[1024];
/* kernel thread initialization */
_gtpuah_sock.running = 1;
PR_INFO(MODULE_NAME": listening on port %d\n", gtpu_sgw_port);
/* main loop */
while(_gtpuah_sock.thread_stop_requested == 0){
if (kthread_should_stop()) {
_gtpuah_sock.running = 0;
PR_INFO(MODULE_NAME": kthread_stop initiated exit at %lu \n", jiffies);
return -1; //Exit from the thread. Return value will be passed to kthread_stop()
}
#if defined(THREAD_SOCK_NO_WAIT)
usleep_range(500,2000);
#endif
size = _gtpuah_ksocket_receive(_gtpuah_sock.sock, &_gtpuah_sock.addr, buf, bufsize);
if (size <= 0) {
if (size != -EAGAIN) {
pr_info(MODULE_NAME": error getting datagram, sock_recvmsg error = %d\n", size);
}
} else {
PR_INFO(MODULE_NAME": received %d bytes\n", size);
if ((tx_size = _gtpuah_ksocket_process_gtp(buf, size, gtp_resp)) > 0) {
//ksocket_send(_gtpuah_sock.sock, &_gtpuah_sock.addr_send, buf, gtp_resp, tx_size, NULL, 0));
}
}
}
_gtpuah_sock.running = 0;
if (kthread_should_stop()) {
PR_INFO(MODULE_NAME": kthread_stop initiated exit at %lu \n", jiffies);
return -1; //Exit from the thread. Return value will be passed to kthread_stop()
}
PR_INFO(MODULE_NAME": kthread do_exit()\n");
do_exit(0);
}
//-----------------------------------------------------------------------------
static int _gtpuah_ksocket_process_gtp(const unsigned char * const rx_buf_pP, const int lenP, unsigned char* tx_buf_pP)
//-----------------------------------------------------------------------------
{
gtpv1u_msg_t gtpv1u_msg;
uint8_t msg_type;
struct iphdr *iph_p = NULL;
struct iphdr *new_iph_p= NULL;
struct sk_buff *skb_p = NULL;
const unsigned char * rx_buf_p = rx_buf_pP;
int err = 0;
struct rtable *rt = NULL;
struct flowi fl = {
.u = {
.ip4 = {
.daddr = 0,
.flowi4_tos = 0,
.flowi4_scope = RT_SCOPE_UNIVERSE,
}
}
};
msg_type = rx_buf_pP[1];
switch(msg_type) {
case GTP_ECHO_REQ:
PR_INFO(MODULE_NAME": TODO GTP ECHO_REQ, SEND TO GTPV1U TASK USER SPACE\n");
//TODO;
return 0;
break;
case GTP_ERROR_INDICATION:
PR_INFO(MODULE_NAME":TODO GTP ERROR INDICATION, SEND TO GTPV1U TASK USER SPACE\n");
//TODO;
return 0;
break;
case GTP_ECHO_RSP:
PR_INFO(MODULE_NAME":GTP ECHO_RSP, SEND TO GTPV1U TASK USER SPACE\n");
return 0;
break;
case GTP_GPDU: {
gtpv1u_msg.version = ((*rx_buf_p) & 0xE0) >> 5;
gtpv1u_msg.protocol_type = ((*rx_buf_p) & 0x10) >> 4;
gtpv1u_msg.ext_hdr_flag = ((*rx_buf_p) & 0x04) >> 2;
gtpv1u_msg.seq_num_flag = ((*rx_buf_p) & 0x02) >> 1;
gtpv1u_msg.npdu_num_flag = ((*rx_buf_p) & 0x01);
rx_buf_p++;
gtpv1u_msg.msg_type = *(rx_buf_p);
rx_buf_p++;
rx_buf_p += 2;
gtpv1u_msg.teid = ntohl(*((u_int32_t *)rx_buf_p));
rx_buf_p += 4;
if(gtpv1u_msg.ext_hdr_flag || gtpv1u_msg.seq_num_flag || gtpv1u_msg.npdu_num_flag) {
gtpv1u_msg.seq_num = ntohs(*(((u_int16_t *)rx_buf_p)));
rx_buf_p += 2;
gtpv1u_msg.npdu_num = *(rx_buf_p++);
gtpv1u_msg.next_ext_hdr_type = *(rx_buf_p++);
}
gtpv1u_msg.msg_buf_offset = (u_int32_t)(rx_buf_p - rx_buf_pP);
gtpv1u_msg.msg_buf_len = lenP - gtpv1u_msg.msg_buf_offset;
gtpv1u_msg.msg_len = lenP;
iph_p = (struct iphdr*)(&rx_buf_pP[gtpv1u_msg.msg_buf_offset]);
fl.u.ip4.daddr = iph_p->daddr;
fl.u.ip4.flowi4_tos = RT_TOS(iph_p->tos);
rt = ip_route_output_key(&init_net, &fl.u.ip4);
if (rt == NULL) {
PR_INFO("GTPURH: Failed to route packet to dst 0x%x. Error: (%d)\n", fl.u.ip4.daddr, err);
return NF_DROP;
}
if (rt->dst.dev == NULL) {
pr_info("GTPURH: dst dev NULL\n");
return 0;
}
skb_p = alloc_skb(LL_MAX_HEADER + ntohs(iph_p->tot_len), GFP_ATOMIC);
if (skb_p == NULL) {
return 0;
}
skb_p->priority = rt_tos2priority(iph_p->tos);
skb_p->pkt_type = PACKET_OTHERHOST;
skb_dst_set(skb_p, dst_clone(&rt->dst));
skb_p->dev = skb_dst(skb_p)->dev;
skb_reserve(skb_p, LL_MAX_HEADER + ntohs(iph_p->tot_len));
skb_p->protocol = htons(ETH_P_IP);
new_iph_p = (void *)skb_push(skb_p, ntohs(iph_p->tot_len) - (iph_p->ihl << 2));
skb_reset_transport_header(skb_p);
new_iph_p = (void *)skb_push(skb_p, iph_p->ihl << 2);
memcpy(new_iph_p, iph_p, ntohs(iph_p->tot_len));
skb_reset_network_header(skb_p);
skb_reset_inner_network_header(skb_p);
skb_reset_inner_transport_header(skb_p);
skb_p->mark = gtpv1u_msg.teid;
new_iph_p->ttl = ip4_dst_hoplimit(skb_dst(skb_p));
skb_p->ip_summed = CHECKSUM_NONE;
if (skb_p->len > dst_mtu(skb_dst(skb_p))) {
PR_INFO("GTPURH: bad length\n");
goto free_skb;
}
ip_local_out(skb_p);
return 0;
free_skb:
pr_info("GTPURH: Dropped skb\n");
kfree_skb(skb_p);
return 0;
}
break;
default:
PR_INFO(MODULE_NAME":ERROR GTPU msg type %u\n", msg_type);
return 0;
}
}
//-----------------------------------------------------------------------------
static int _gtpuah_ksocket_receive(struct socket* sock_pP, struct sockaddr_in* addr_pP, unsigned char* buf_pP, int lenP)
//-----------------------------------------------------------------------------
{
struct msghdr msg;
struct iovec iov;
mm_segment_t oldfs;
int size = 0;
if (sock_pP->sk==NULL) return 0;
iov.iov_base = buf_pP;
iov.iov_len = lenP;
#if defined(THREAD_SOCK_NO_WAIT)
msg.msg_flags = MSG_DONTWAIT;
#else
msg.msg_flags = 0;
#endif
msg.msg_name = addr_pP;
msg.msg_namelen = sizeof(struct sockaddr_in);
msg.msg_control = NULL;
msg.msg_controllen = 0;
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_control = NULL;
oldfs = get_fs();
set_fs(KERNEL_DS);
size = sock_recvmsg(sock_pP,&msg,lenP,msg.msg_flags);
set_fs(oldfs);
return size;
}
//-----------------------------------------------------------------------------
static int _gtpuah_ksocket_send(struct socket *sock_pP, struct sockaddr_in *addr_pP, unsigned char *gtpuh_pP, int len_gtpP, unsigned char *buf_ip_pP, int len_ipP)
//-----------------------------------------------------------------------------
{
struct msghdr msg;
struct iovec iov[2];
mm_segment_t oldfs;
int size = 0;
int err = 0;
int iov_index = 0;
if ( (err = sock_pP->ops->connect(sock_pP, (struct sockaddr *)addr_pP, sizeof(struct sockaddr), 0)) < 0 ) {
PR_INFO(MODULE_NAME": Could not connect to socket, error = %d\n", -err);
return 0;
}
if (sock_pP->sk == NULL) {
return 0;
}
if ((gtpuh_pP != NULL) && (len_gtpP > 0)) {
iov[iov_index].iov_base = gtpuh_pP;
iov[iov_index].iov_len = len_gtpP;
iov_index += 1;
}
if ((buf_ip_pP != NULL) && (len_ipP > 0)) {
iov[iov_index].iov_base = buf_ip_pP;
iov[iov_index].iov_len = len_ipP;
iov_index += 1;
}
msg.msg_flags = 0;
msg.msg_name = addr_pP;
msg.msg_namelen = sizeof(struct sockaddr_in);
msg.msg_control = NULL;
msg.msg_controllen = 0;
msg.msg_iov = iov;
msg.msg_iovlen = iov_index;
msg.msg_control = NULL;
oldfs = get_fs();
set_fs(KERNEL_DS);
size = sock_sendmsg(sock_pP,&msg,len_ipP+len_gtpP);
set_fs(oldfs);
return size;
}
#ifdef WITH_IPV6
//-----------------------------------------------------------------------------
static void
_gtpuah_tg6_add(struct sk_buff *old_skb_pP, const struct xt_action_param *par_pP)
{
//-----------------------------------------------------------------------------
}
#endif
//-----------------------------------------------------------------------------
static void
_gtpuah_tg4_add(struct sk_buff *old_skb_pP, const struct xt_action_param *par_pP)
{
//-----------------------------------------------------------------------------
struct iphdr *old_iph_p = ip_hdr(old_skb_pP);
struct gtpuhdr gtpuh;
uint16_t orig_iplen = 0;
// CONNMARK
enum ip_conntrack_info ctinfo;
struct nf_conn *ct = NULL;
u_int32_t newmark;
if (skb_linearize(old_skb_pP) < 0) {
PR_INFO(MODULE_NAME": skb no linearize\n");
return;
}
//----------------------------------------------------------------------------
// CONNMARK
//----------------------------------------------------------------------------
ct = nf_ct_get(old_skb_pP, &ctinfo);
if (ct == NULL) {
PR_INFO(MODULE_NAME": _gtpuah_target_add force info_pP mark %u to skb_pP mark %u\n",
old_skb_pP->mark,
((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->rtun);
old_skb_pP->mark = ((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->rtun;
} else {
//XT_CONNMARK_RESTORE:
newmark = old_skb_pP->mark ^ ct->mark;
PR_INFO(MODULE_NAME": _gtpuah_target_add restore mark %u (skb mark %u ct mark %u) sgw addr %x\n",
newmark, old_skb_pP->mark, ct->mark,
((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->raddr);
}
orig_iplen = ntohs(old_iph_p->tot_len);
/* Add GTPu header */
gtpuh.flags = 0x30; /* v1 and Protocol-type=GTP */
gtpuh.msgtype = 0xff; /* T-PDU */
gtpuh.length = htons(orig_iplen);
gtpuh.tunid = htonl(newmark);
_gtpuah_sock.addr_send.sin_addr.s_addr = ((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->raddr;
_gtpuah_ksocket_send(_gtpuah_sock.sock, &_gtpuah_sock.addr_send, (unsigned char*)&gtpuh, sizeof(gtpuh), (unsigned char*)old_iph_p, orig_iplen);
return ;
}
#ifdef WITH_IPV6
//-----------------------------------------------------------------------------
static unsigned int
_gtpuah_tg6(struct sk_buff *skb_pP, const struct xt_action_param *par_pP)
{
//-----------------------------------------------------------------------------
const struct xt_gtpuah_target_info *tgi_p = par_pP->targinfo;
if (tgi_p == NULL) {
return NF_ACCEPT;
}
if (tgi_p->action == PARAM_GTPUAH_ACTION_ADD) {
_gtpuah_tg6_add(skb_pP, par_pP);
return NF_DROP; // TODO
}
return NF_ACCEPT;
}
#endif
//-----------------------------------------------------------------------------
static unsigned int
_gtpuah_tg4(struct sk_buff *skb_pP, const struct xt_action_param *par_pP)
{
//-----------------------------------------------------------------------------
const struct xt_gtpuah_target_info *tgi_p = par_pP->targinfo;
if (tgi_p == NULL) {
return NF_ACCEPT;
}
if (tgi_p->action == PARAM_GTPUAH_ACTION_ADD) {
_gtpuah_tg4_add(skb_pP, par_pP);
return NF_DROP;
}
return NF_ACCEPT;
}
//-----------------------------------------------------------------------------
static int
__init gtpuah_tg_init(void)
//-----------------------------------------------------------------------------
{
int err;
pr_info(MODULE_NAME": Initializing module (KVersion: %d)\n", KVERSION);
pr_info(MODULE_NAME": Copyright Polaris Networks 2010-2011\n");
pr_info(MODULE_NAME": Modified by EURECOM Lionel GAUTHIER 2014\n");
#ifndef CMAKER
pr_info(MODULE_NAME": Compiled %s at time %s\n",__DATE__,__TIME__);
#endif
#if defined(WITH_IPV6)
pr_info(MODULE_NAME": IPv4/IPv6 enabled\n");
#else
pr_info(MODULE_NAME": IPv4 only enabled\n");
#endif
pr_info(MODULE_NAME": params gtpu_enb_port=%u, gtpu_sgw_port=%u, sgw_addr=%s\n",
gtpu_enb_port, gtpu_sgw_port, sgw_addr);
// UDP socket socket
memset(&_gtpuah_sock, 0, sizeof(gtpuah_sock_t));
/* create a socket */
if ((err = sock_create(AF_INET, SOCK_DGRAM, IPPROTO_UDP, &_gtpuah_sock.sock)) < 0 ) {
PR_INFO(": Could not create a datagram socket, error = %d\n", -ENXIO);
return err;
}
_gtpuah_sock.addr.sin_family = AF_INET;
_gtpuah_sock.addr.sin_port = htons(gtpu_sgw_port);
_gtpuah_sock.addr.sin_addr.s_addr = in_aton(sgw_addr);
_gtpuah_sock.addr_send.sin_family = AF_INET;
_gtpuah_sock.addr_send.sin_port = htons(gtpu_enb_port);
_gtpuah_sock.addr_send.sin_addr.s_addr = in_aton(sgw_addr);
_gtpuah_sock.thread_stop_requested = 0;
if ( (err = _gtpuah_sock.sock->ops->bind(_gtpuah_sock.sock, (struct sockaddr *)&_gtpuah_sock.addr, sizeof(struct sockaddr) ) ) < 0) {
pr_info(MODULE_NAME": Could not bind socket, error = %d\n", -err);
goto close_and_out;
}
if ( (err = _gtpuah_sock.sock->ops->connect(_gtpuah_sock.sock, (struct sockaddr *)&_gtpuah_sock.addr_send, sizeof(struct sockaddr), 0)) < 0 ) {
pr_info(MODULE_NAME": Could not connect to socket, error = %d\n", -err);
goto close_and_out;
}
// start kernel thread
_gtpuah_sock.thread = kthread_run((void *)_udp_thread, NULL, MODULE_NAME);
if (IS_ERR(_gtpuah_sock.thread)) {
pr_info(MODULE_NAME": unable to start kernel thread\n");
return -ENOMEM;
}
if((_gtpuah_sock.thread)) {
wake_up_process(_gtpuah_sock.thread);
}
return xt_register_targets(gtpuah_tg_reg, ARRAY_SIZE(gtpuah_tg_reg));
close_and_out:
sock_release(_gtpuah_sock.sock);
_gtpuah_sock.sock = NULL;
return err;
}
//-----------------------------------------------------------------------------
static void
__exit gtpuah_tg_exit(void)
//-----------------------------------------------------------------------------
{
int err;
int loop = 0;
if (_gtpuah_sock.thread==NULL) {
pr_info(MODULE_NAME": no kernel thread to kill\n");
} else {
if (_gtpuah_sock.running > 0) {
_gtpuah_sock.thread_stop_requested = 1;
pr_info(MODULE_NAME": exit kernel thread requested\n");
do {
pr_info(MODULE_NAME": waking up thread with datagram\n");
msleep(5);
pr_info(MODULE_NAME": waiting for thread...\n");
loop++;
} while ((_gtpuah_sock.running > 0) && (loop < 20));
if (_gtpuah_sock.running > 0) {
pr_info(MODULE_NAME": stopping kernel thread\n");
err = kthread_stop(_gtpuah_sock.thread);
if(!err) {
pr_info(MODULE_NAME": Successfully killed kernel thread!\n");
} else {
pr_info(MODULE_NAME": Unsuccessfully killed kernel thread!\n");
}
} else {
pr_info(MODULE_NAME": Found thread exited by itself\n");
}
}
}
/* free allocated resources before exit */
if (_gtpuah_sock.sock != NULL) {
sock_release(_gtpuah_sock.sock);
_gtpuah_sock.sock = NULL;
}
xt_unregister_targets(gtpuah_tg_reg, ARRAY_SIZE(gtpuah_tg_reg));
pr_info(MODULE_NAME": Unloading module\n");
}
module_init(gtpuah_tg_init);
module_exit(gtpuah_tg_exit);
MODULE_ALIAS("ipt6_GTPUAH");
MODULE_ALIAS("ipt_GTPUAH");
/*
* GTPu klm for Linux/iptables
*
* Copyright (c) 2010-2011 Polaris Networks
* Author: Pradip Biswas <pradip_biswas@polarisnetworks.net>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
*/
#include <linux/module.h>
#include <linux/kallsyms.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/skbuff.h>
#include <linux/ip.h>
#include <linux/if_ether.h>
#include <linux/route.h>
#include <linux/time.h>
#include <net/checksum.h>
#include <net/ip.h>
#include <linux/in.h>
#include <linux/icmp.h>
#include <net/udp.h>
#include <net/inet_sock.h>
#include <net/route.h>
#include <net/addrconf.h>
#include <net/ip6_checksum.h>
#include <net/ip6_route.h>
#include <net/ipv6.h>
#include <linux/netfilter/x_tables.h>
#include <linux/netfilter_ipv4/ip_tables.h>
#include <linux/kthread.h>
#include <linux/errno.h>
#include <linux/types.h>
#include <linux/netdevice.h>
#include <linux/delay.h>
#include <linux/inet.h>
// CONNMARK
#include <net/netfilter/nf_conntrack.h>
#include <net/netfilter/nf_conntrack_ecache.h>
#include <linux/netfilter/x_tables.h>
#include <linux/netfilter/xt_connmark.h>
#if defined(CONFIG_IP6_NF_IPTABLES) || defined(CONFIG_IP6_NF_IPTABLES_MODULE)
# define WITH_IPV6 1
#endif
#include "xt_GTPUAH.h"
#if !(defined KVERSION)
# error "Kernel version is not defined!!!! Exiting."
#endif
#undef THREAD_SOCK_NO_WAIT
#define TRACE_IN_KERN_LOG 1
#define TRACE_ICMP_IN_KERN_LOG 1
#if defined(TRACE_IN_KERN_LOG)
#define PR_INFO(fORMAT, aRGS...) pr_info(fORMAT, ##aRGS)
#else
#define PR_INFO(fORMAT, aRGS...)
#endif
#define INT_MODULE_PARM(n, v) static int n = v; module_param(n, int, 0444)
#define STRING_MODULE_PARM(s, v) static char* s = v; module_param(s, charp, 0000);
//-----------------------------------------------------------------------------
static char* _gtpuah_nf_inet_hook_2_string(int nf_inet_hookP);
static void _gtpuah_print_hex_octets(unsigned char* data_pP, unsigned short sizeP);
static void _gtpuah_tg4_add(struct sk_buff *old_skb_pP, const struct xt_action_param *par_pP);
#ifdef WITH_IPV6
static void _gtpuah_tg6_add(struct sk_buff *old_skb_pP, const struct xt_action_param *par_pP);
static unsigned int _gtpuah_tg6(struct sk_buff *skb_pP, const struct xt_action_param *par_pP);
#endif
static unsigned int _gtpuah_tg4(struct sk_buff *skb_pP, const struct xt_action_param *par_pP);
static int __init gtpuah_tg_init(void);
static void __exit gtpuah_tg_exit(void);
static int _udp_thread(void *data);
static int _gtpuah_ksocket_send(struct socket *sock_pP, struct sockaddr_in *addr_pP, unsigned char *gtpuh_pP, int len_gtpP, unsigned char *buf_ip_pP, int len_ipP);
static int _gtpuah_ksocket_receive(struct socket* sock_pP, struct sockaddr_in* addr_pP, unsigned char* buf_pP, int lenP);
static int _gtpuah_ksocket_process_gtp(const unsigned char * const rx_buf_pP, const int lenP, unsigned char* tx_buf_pP);
//-----------------------------------------------------------------------------
#define MODULE_NAME "GTPUAH"
MODULE_LICENSE("GPL");
MODULE_AUTHOR("Pradip Biswas <pradip_biswas@polarisnetworks.net>");
MODULE_DESCRIPTION("GTPu Data Path extension on netfilter");
//-----------------------------------------------------------------------------
static struct xt_target gtpuah_tg_reg[] __read_mostly = {
{
.name = MODULE_NAME,
.revision = 0,
.family = NFPROTO_IPV4,
.hooks = (1 << NF_INET_POST_ROUTING) |
(1 << NF_INET_LOCAL_IN) |
(1 << NF_INET_FORWARD),
.table = "mangle",
.target = _gtpuah_tg4,
.targetsize = sizeof(struct xt_gtpuah_target_info),
.me = THIS_MODULE,
},
#ifdef WITH_IPV6
{
.name = MODULE_NAME,
.revision = 0,
.family = NFPROTO_IPV6,
.hooks = (1 << NF_INET_POST_ROUTING) |
(1 << NF_INET_LOCAL_IN) |
(1 << NF_INET_FORWARD),
.table = "mangle",
.target = _gtpuah_tg6,
.targetsize = sizeof(struct xt_gtpuah_target_info),
.me = THIS_MODULE,
},
#endif
};
#define GTP_ECHO_REQ 1
#define GTP_ECHO_RSP 2
#define GTP_ERROR_INDICATION 26
#define GTP_GPDU 255
typedef struct gtpv1u_msg_s {
unsigned char version;
unsigned char protocol_type;
unsigned char ext_hdr_flag;
unsigned char seq_num_flag;
u_int16_t npdu_num_flag;
u_int32_t msg_type;
u_int16_t msg_len;
u_int32_t teid;
u_int16_t seq_num;
unsigned char npdu_num;
unsigned char next_ext_hdr_type;
u_int32_t msg_buf_len;
u_int32_t msg_buf_offset;
struct gtpv1u_msg_s* next;
} gtpv1u_msg_t;
struct gtpuhdr {
char flags;
char msgtype;
u_int16_t length;
u_int32_t tunid;
};
typedef struct gtpuah_sock_s {
struct task_struct *thread;
struct sockaddr_in addr;
struct socket *sock;
struct sockaddr_in addr_send;
int running;
int thread_stop_requested;
} gtpuah_sock_t;
//-----------------------------------------------------------------------------
#define GTPU_HDR_PNBIT 1
#define GTPU_HDR_SBIT 1 << 1
#define GTPU_HDR_EBIT 1 << 2
#define GTPU_ANY_EXT_HDR_BIT (GTPU_HDR_PNBIT | GTPU_HDR_SBIT | GTPU_HDR_EBIT)
#define GTPU_FAILURE 1
#define GTPU_SUCCESS !GTPU_FAILURE
#define GTPUAH_2_PRINT_BUFFER_LEN 8192
#define IP_MORE_FRAGMENTS 0x2000
#define NIPADDR(addr) \
(uint8_t)(addr & 0x000000FF), \
(uint8_t)((addr & 0x0000FF00) >> 8), \
(uint8_t)((addr & 0x00FF0000) >> 16), \
(uint8_t)((addr & 0xFF000000) >> 24)
//-----------------------------------------------------------------------------
static char _gtpuah_print_buffer[GTPUAH_2_PRINT_BUFFER_LEN];
gtpuah_sock_t _gtpuah_sock;
INT_MODULE_PARM(gtpu_sgw_port, 2152);
MODULE_PARM_DESC(gtpu_sgw_port, "UDP port number for S1U interface (s-GW side)");
INT_MODULE_PARM(gtpu_enb_port, 2153);
MODULE_PARM_DESC(gtpu_enb_port, "UDP port number for S1U interface (eNB side)");
STRING_MODULE_PARM(sgw_addr, "127.0.0.1");
MODULE_PARM_DESC(sgw_addr, "IPv4 address of the S1U IP interface");
//-----------------------------------------------------------------------------
static char*
_gtpuah_icmph_type_2_string(uint8_t typeP)
//-----------------------------------------------------------------------------
{
switch (typeP) {
case ICMP_ECHOREPLY:return "ECHOREPLY";break;
case ICMP_DEST_UNREACH:return "DEST_UNREACH";break;
case ICMP_SOURCE_QUENCH:return "SOURCE_QUENCH";break;
case ICMP_REDIRECT:return "REDIRECT";break;
case ICMP_ECHO:return "ECHO";break;
case ICMP_TIME_EXCEEDED:return "TIME_EXCEEDED";break;
case ICMP_PARAMETERPROB:return "PARAMETERPROB";break;
case ICMP_TIMESTAMP:return "TIMESTAMP";break;
case ICMP_TIMESTAMPREPLY:return "TIMESTAMPREPLY";break;
case ICMP_INFO_REQUEST:return "INFO_REQUEST";break;
case ICMP_INFO_REPLY:return "INFO_REPLY";break;
case ICMP_ADDRESS:return "ADDRESS";break;
case ICMP_ADDRESSREPLY:return "ADDRESSREPLY";break;
default:return "TYPE?";
}
}
//-----------------------------------------------------------------------------
static char*
_gtpuah_nf_inet_hook_2_string(int nf_inet_hookP)
{
//-----------------------------------------------------------------------------
switch (nf_inet_hookP) {
case NF_INET_PRE_ROUTING:
return "NF_INET_PRE_ROUTING";
break;
case NF_INET_LOCAL_IN:
return "NF_INET_LOCAL_IN";
break;
case NF_INET_FORWARD:
return "NF_INET_FORWARD";
break;
case NF_INET_LOCAL_OUT:
return "NF_INET_LOCAL_OUT";
break;
case NF_INET_POST_ROUTING:
return "NF_INET_POST_ROUTING";
break;
default:
return "NF_INET_UNKNOWN";
}
}
//-----------------------------------------------------------------------------
void
_gtpuah_print_hex_octets(unsigned char* data_pP, 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 (data_pP == NULL) {
return;
}
if (sizeP > 2000) {
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.%06ld", h,m,s,tv.tv_usec);
buffer_marker+=snprintf(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker,"%s------+-------------------------------------------------+\n",timeofday);
buffer_marker+=snprintf(&_gtpuah_print_buffer[buffer_marker], GTPUAH_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(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker,"%s------+-------------------------------------------------+\n",timeofday);
pr_info("%s",_gtpuah_print_buffer);
buffer_marker = 0;
for (octet_index = 0; octet_index < sizeP; octet_index++) {
if ((octet_index % 16) == 0) {
if (octet_index != 0) {
buffer_marker+=snprintf(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker, " |\n");
pr_info("%s",_gtpuah_print_buffer);
buffer_marker = 0;
}
buffer_marker+=snprintf(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker, "%s %04ld |",timeofday, octet_index);
}
/*
* Print every single octet in hexadecimal form
*/
buffer_marker+=snprintf(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker, " %02x", data_pP[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(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker, " ");
//SGI_IF_DEBUG(" ");
buffer_marker+=snprintf(&_gtpuah_print_buffer[buffer_marker], GTPUAH_2_PRINT_BUFFER_LEN - buffer_marker, " |\n");
pr_info("%s",_gtpuah_print_buffer);
}
// for uplink GTPU traffic on S-GW
//-----------------------------------------------------------------------------
static int _udp_thread(void *data)
//-----------------------------------------------------------------------------
{
int size, tx_size;
int bufsize = 8192;
int success_read = 0;
int failed_read = 0;
unsigned char buf[bufsize+1];
unsigned char gtp_resp[1024];
/* kernel thread initialization */
_gtpuah_sock.running = 1;
PR_INFO(MODULE_NAME": listening on port %d\n", gtpu_sgw_port);
/* main loop */
while(_gtpuah_sock.thread_stop_requested == 0){
if (kthread_should_stop()) {
_gtpuah_sock.running = 0;
PR_INFO(MODULE_NAME": kthread_stop initiated exit at %lu \n", jiffies);
return -1; //Exit from the thread. Return value will be passed to kthread_stop()
}
size = _gtpuah_ksocket_receive(_gtpuah_sock.sock, &_gtpuah_sock.addr, buf, bufsize);
if (size <= 0) {
success_read = 0;
failed_read += 1;
if (failed_read > 10) failed_read = 10;
if (size != -EAGAIN) {
pr_info(MODULE_NAME": error getting datagram, sock_recvmsg error = %d\n", size);
}
#if defined(THREAD_SOCK_NO_WAIT)
usleep_range(failed_read*20,failed_read*200);
#endif
} else {
success_read += 1;
failed_read = 0;
PR_INFO(MODULE_NAME": received %d bytes\n", size);
if ((tx_size = _gtpuah_ksocket_process_gtp(buf, size, gtp_resp)) > 0) {
//ksocket_send(_gtpuah_sock.sock, &_gtpuah_sock.addr_send, buf, gtp_resp, tx_size, NULL, 0));
}
}
}
_gtpuah_sock.running = 0;
if (kthread_should_stop()) {
PR_INFO(MODULE_NAME": kthread_stop initiated exit at %lu \n", jiffies);
return -1; //Exit from the thread. Return value will be passed to kthread_stop()
}
PR_INFO(MODULE_NAME": kthread do_exit()\n");
do_exit(0);
}
//-----------------------------------------------------------------------------
static int _gtpuah_ksocket_process_gtp(const unsigned char * const rx_buf_pP, const int lenP, unsigned char* tx_buf_pP)
//-----------------------------------------------------------------------------
{
gtpv1u_msg_t gtpv1u_msg;
uint8_t msg_type;
struct iphdr *iph_p = NULL;
struct iphdr *new_iph_p= NULL;
struct sk_buff *skb_p = NULL;
const unsigned char * rx_buf_p = rx_buf_pP;
int err = 0;
struct rtable *rt = NULL;
struct flowi fl = {
.u = {
.ip4 = {
.daddr = 0,
.flowi4_tos = 0,
.flowi4_scope = RT_SCOPE_UNIVERSE,
}
}
};
msg_type = rx_buf_pP[1];
switch(msg_type) {
case GTP_ECHO_REQ:
PR_INFO(MODULE_NAME": TODO GTP ECHO_REQ, SEND TO GTPV1U TASK USER SPACE\n");
//TODO;
return 0;
break;
case GTP_ERROR_INDICATION:
PR_INFO(MODULE_NAME":TODO GTP ERROR INDICATION, SEND TO GTPV1U TASK USER SPACE\n");
//TODO;
return 0;
break;
case GTP_ECHO_RSP:
PR_INFO(MODULE_NAME":GTP ECHO_RSP, SEND TO GTPV1U TASK USER SPACE\n");
return 0;
break;
case GTP_GPDU: {
gtpv1u_msg.version = ((*rx_buf_p) & 0xE0) >> 5;
gtpv1u_msg.protocol_type = ((*rx_buf_p) & 0x10) >> 4;
gtpv1u_msg.ext_hdr_flag = ((*rx_buf_p) & 0x04) >> 2;
gtpv1u_msg.seq_num_flag = ((*rx_buf_p) & 0x02) >> 1;
gtpv1u_msg.npdu_num_flag = ((*rx_buf_p) & 0x01);
rx_buf_p++;
gtpv1u_msg.msg_type = *(rx_buf_p);
rx_buf_p++;
rx_buf_p += 2;
gtpv1u_msg.teid = ntohl(*((u_int32_t *)rx_buf_p));
rx_buf_p += 4;
if(gtpv1u_msg.ext_hdr_flag || gtpv1u_msg.seq_num_flag || gtpv1u_msg.npdu_num_flag) {
gtpv1u_msg.seq_num = ntohs(*(((u_int16_t *)rx_buf_p)));
rx_buf_p += 2;
gtpv1u_msg.npdu_num = *(rx_buf_p++);
gtpv1u_msg.next_ext_hdr_type = *(rx_buf_p++);
}
gtpv1u_msg.msg_buf_offset = (u_int32_t)(rx_buf_p - rx_buf_pP);
gtpv1u_msg.msg_buf_len = lenP - gtpv1u_msg.msg_buf_offset;
gtpv1u_msg.msg_len = lenP;
iph_p = (struct iphdr*)(&rx_buf_pP[gtpv1u_msg.msg_buf_offset]);
fl.u.ip4.daddr = iph_p->daddr;
fl.u.ip4.flowi4_tos = RT_TOS(iph_p->tos);
rt = ip_route_output_key(&init_net, &fl.u.ip4);
if (rt == NULL) {
PR_INFO("GTPURH: Failed to route packet to dst 0x%x. Error: (%d)\n", fl.u.ip4.daddr, err);
return NF_DROP;
}
if (rt->dst.dev == NULL) {
pr_info("GTPURH: dst dev NULL\n");
return 0;
}
skb_p = alloc_skb(LL_MAX_HEADER + ntohs(iph_p->tot_len), GFP_ATOMIC);
if (skb_p == NULL) {
return 0;
}
skb_p->priority = rt_tos2priority(iph_p->tos);
skb_p->pkt_type = PACKET_OTHERHOST;
skb_dst_set(skb_p, dst_clone(&rt->dst));
skb_p->dev = skb_dst(skb_p)->dev;
skb_reserve(skb_p, LL_MAX_HEADER + ntohs(iph_p->tot_len));
skb_p->protocol = htons(ETH_P_IP);
new_iph_p = (void *)skb_push(skb_p, ntohs(iph_p->tot_len) - (iph_p->ihl << 2));
skb_reset_transport_header(skb_p);
new_iph_p = (void *)skb_push(skb_p, iph_p->ihl << 2);
memcpy(new_iph_p, iph_p, ntohs(iph_p->tot_len));
skb_reset_network_header(skb_p);
skb_reset_inner_network_header(skb_p);
skb_reset_inner_transport_header(skb_p);
skb_p->mark = gtpv1u_msg.teid;
new_iph_p->ttl = ip4_dst_hoplimit(skb_dst(skb_p));
skb_p->ip_summed = CHECKSUM_NONE;
if (skb_p->len > dst_mtu(skb_dst(skb_p))) {
PR_INFO("GTPURH: bad length\n");
goto free_skb;
}
ip_local_out(skb_p);
return 0;
free_skb:
pr_info("GTPURH: Dropped skb\n");
kfree_skb(skb_p);
return 0;
}
break;
default:
PR_INFO(MODULE_NAME":ERROR GTPU msg type %u\n", msg_type);
return 0;
}
}
//-----------------------------------------------------------------------------
static int _gtpuah_ksocket_receive(struct socket* sock_pP, struct sockaddr_in* addr_pP, unsigned char* buf_pP, int lenP)
//-----------------------------------------------------------------------------
{
struct msghdr msg;
struct iovec iov;
mm_segment_t oldfs;
int size = 0;
if (sock_pP->sk==NULL) return 0;
iov.iov_base = buf_pP;
iov.iov_len = lenP;
#if defined(THREAD_SOCK_NO_WAIT)
msg.msg_flags = MSG_DONTWAIT;
#else
msg.msg_flags = 0;
#endif
msg.msg_name = addr_pP;
msg.msg_namelen = sizeof(struct sockaddr_in);
msg.msg_control = NULL;
msg.msg_controllen = 0;
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_control = NULL;
oldfs = get_fs();
set_fs(KERNEL_DS);
size = sock_recvmsg(sock_pP,&msg,lenP,msg.msg_flags);
set_fs(oldfs);
return size;
}
//-----------------------------------------------------------------------------
static int _gtpuah_ksocket_send(struct socket *sock_pP, struct sockaddr_in *addr_pP, unsigned char *gtpuh_pP, int len_gtpP, unsigned char *buf_ip_pP, int len_ipP)
//-----------------------------------------------------------------------------
{
struct msghdr msg;
struct iovec iov[2];
mm_segment_t oldfs;
int size = 0;
int err = 0;
int iov_index = 0;
if ( (err = sock_pP->ops->connect(sock_pP, (struct sockaddr *)addr_pP, sizeof(struct sockaddr), 0)) < 0 ) {
PR_INFO(MODULE_NAME": Could not connect to socket, error = %d\n", -err);
return 0;
}
if (sock_pP->sk == NULL) {
return 0;
}
if ((gtpuh_pP != NULL) && (len_gtpP > 0)) {
iov[iov_index].iov_base = gtpuh_pP;
iov[iov_index].iov_len = len_gtpP;
iov_index += 1;
}
if ((buf_ip_pP != NULL) && (len_ipP > 0)) {
iov[iov_index].iov_base = buf_ip_pP;
iov[iov_index].iov_len = len_ipP;
iov_index += 1;
}
msg.msg_flags = 0;
msg.msg_name = addr_pP;
msg.msg_namelen = sizeof(struct sockaddr_in);
msg.msg_control = NULL;
msg.msg_controllen = 0;
msg.msg_iov = iov;
msg.msg_iovlen = iov_index;
msg.msg_control = NULL;
oldfs = get_fs();
set_fs(KERNEL_DS);
size = sock_sendmsg(sock_pP,&msg,len_ipP+len_gtpP);
set_fs(oldfs);
return size;
}
#ifdef WITH_IPV6
//-----------------------------------------------------------------------------
static void
_gtpuah_tg6_add(struct sk_buff *old_skb_pP, const struct xt_action_param *par_pP)
{
//-----------------------------------------------------------------------------
}
#endif
//-----------------------------------------------------------------------------
static void
_gtpuah_tg4_add(struct sk_buff *old_skb_pP, const struct xt_action_param *par_pP)
{
//-----------------------------------------------------------------------------
struct iphdr *old_iph_p = ip_hdr(old_skb_pP);
struct gtpuhdr gtpuh;
uint16_t orig_iplen = 0;
// CONNMARK
enum ip_conntrack_info ctinfo;
struct nf_conn *ct = NULL;
u_int32_t newmark;
if (skb_linearize(old_skb_pP) < 0) {
PR_INFO(MODULE_NAME": skb no linearize\n");
return;
}
orig_iplen = ntohs(old_iph_p->tot_len);
//----------------------------------------------------------------------------
// CONNMARK
//----------------------------------------------------------------------------
ct = nf_ct_get(old_skb_pP, &ctinfo);
if (ct == NULL) {
PR_INFO(MODULE_NAME": _gtpuah_target_add force info_pP mark %u to skb_pP mark %u\n",
old_skb_pP->mark,
((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->rtun);
old_skb_pP->mark = ((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->rtun;
} else {
//XT_CONNMARK_RESTORE:
newmark = old_skb_pP->mark ^ ct->mark;
PR_INFO(MODULE_NAME": _gtpuah_target_add restore mark %u (skb mark %u ct mark %u) len %u sgw addr %x\n",
newmark, old_skb_pP->mark, ct->mark, orig_iplen,
((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->raddr);
}
/* Add GTPu header */
gtpuh.flags = 0x30; /* v1 and Protocol-type=GTP */
gtpuh.msgtype = 0xff; /* T-PDU */
gtpuh.length = htons(orig_iplen);
gtpuh.tunid = htonl(newmark);
_gtpuah_sock.addr_send.sin_addr.s_addr = ((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->raddr;
_gtpuah_ksocket_send(_gtpuah_sock.sock, &_gtpuah_sock.addr_send, (unsigned char*)&gtpuh, sizeof(gtpuh), (unsigned char*)old_iph_p, orig_iplen);
return ;
}
#ifdef WITH_IPV6
//-----------------------------------------------------------------------------
static unsigned int
_gtpuah_tg6(struct sk_buff *skb_pP, const struct xt_action_param *par_pP)
{
//-----------------------------------------------------------------------------
const struct xt_gtpuah_target_info *tgi_p = par_pP->targinfo;
if (tgi_p == NULL) {
return NF_ACCEPT;
}
if (tgi_p->action == PARAM_GTPUAH_ACTION_ADD) {
_gtpuah_tg6_add(skb_pP, par_pP);
return NF_DROP; // TODO
}
return NF_ACCEPT;
}
#endif
//-----------------------------------------------------------------------------
static unsigned int
_gtpuah_tg4(struct sk_buff *skb_pP, const struct xt_action_param *par_pP)
{
//-----------------------------------------------------------------------------
const struct xt_gtpuah_target_info *tgi_p = par_pP->targinfo;
if (tgi_p == NULL) {
return NF_ACCEPT;
}
if (tgi_p->action == PARAM_GTPUAH_ACTION_ADD) {
_gtpuah_tg4_add(skb_pP, par_pP);
return NF_DROP;
}
return NF_ACCEPT;
}
//-----------------------------------------------------------------------------
static int
__init gtpuah_tg_init(void)
//-----------------------------------------------------------------------------
{
int err;
pr_info(MODULE_NAME": Initializing module (KVersion: %d)\n", KVERSION);
pr_info(MODULE_NAME": Copyright Polaris Networks 2010-2011\n");
pr_info(MODULE_NAME": Modified by EURECOM Lionel GAUTHIER 2014\n");
#ifndef CMAKER
pr_info(MODULE_NAME": Compiled %s at time %s\n",__DATE__,__TIME__);
#endif
#if defined(WITH_IPV6)
pr_info(MODULE_NAME": IPv4/IPv6 enabled\n");
#else
pr_info(MODULE_NAME": IPv4 only enabled\n");
#endif
pr_info(MODULE_NAME": params gtpu_enb_port=%u, gtpu_sgw_port=%u, sgw_addr=%s\n",
gtpu_enb_port, gtpu_sgw_port, sgw_addr);
// UDP socket socket
memset(&_gtpuah_sock, 0, sizeof(gtpuah_sock_t));
/* create a socket */
if ((err = sock_create(AF_INET, SOCK_DGRAM, IPPROTO_UDP, &_gtpuah_sock.sock)) < 0 ) {
PR_INFO(": Could not create a datagram socket, error = %d\n", -ENXIO);
return err;
}
_gtpuah_sock.addr.sin_family = AF_INET;
_gtpuah_sock.addr.sin_port = htons(gtpu_sgw_port);
_gtpuah_sock.addr.sin_addr.s_addr = in_aton(sgw_addr);
_gtpuah_sock.addr_send.sin_family = AF_INET;
_gtpuah_sock.addr_send.sin_port = htons(gtpu_enb_port);
_gtpuah_sock.addr_send.sin_addr.s_addr = in_aton(sgw_addr);
_gtpuah_sock.thread_stop_requested = 0;
if ( (err = _gtpuah_sock.sock->ops->bind(_gtpuah_sock.sock, (struct sockaddr *)&_gtpuah_sock.addr, sizeof(struct sockaddr) ) ) < 0) {
pr_info(MODULE_NAME": Could not bind socket, error = %d\n", -err);
goto close_and_out;
}
// start kernel thread
_gtpuah_sock.thread = kthread_run((void *)_udp_thread, NULL, MODULE_NAME);
if (IS_ERR(_gtpuah_sock.thread)) {
pr_info(MODULE_NAME": unable to start kernel thread\n");
return -ENOMEM;
}
if((_gtpuah_sock.thread)) {
wake_up_process(_gtpuah_sock.thread);
}
return xt_register_targets(gtpuah_tg_reg, ARRAY_SIZE(gtpuah_tg_reg));
close_and_out:
sock_release(_gtpuah_sock.sock);
_gtpuah_sock.sock = NULL;
return err;
}
//-----------------------------------------------------------------------------
static void
__exit gtpuah_tg_exit(void)
//-----------------------------------------------------------------------------
{
int err;
int loop = 0;
if (_gtpuah_sock.thread==NULL) {
pr_info(MODULE_NAME": no kernel thread to kill\n");
} else {
if (_gtpuah_sock.running > 0) {
_gtpuah_sock.thread_stop_requested = 1;
pr_info(MODULE_NAME": exit kernel thread requested\n");
do {
pr_info(MODULE_NAME": waking up thread with datagram\n");
msleep(5);
pr_info(MODULE_NAME": waiting for thread...\n");
loop++;
} while ((_gtpuah_sock.running > 0) && (loop < 20));
if (_gtpuah_sock.running > 0) {
pr_info(MODULE_NAME": stopping kernel thread\n");
err = kthread_stop(_gtpuah_sock.thread);
if(!err) {
pr_info(MODULE_NAME": Successfully killed kernel thread!\n");
} else {
pr_info(MODULE_NAME": Unsuccessfully killed kernel thread!\n");
}
} else {
pr_info(MODULE_NAME": Found thread exited by itself\n");
}
}
}
/* free allocated resources before exit */
if (_gtpuah_sock.sock != NULL) {
sock_release(_gtpuah_sock.sock);
_gtpuah_sock.sock = NULL;
}
xt_unregister_targets(gtpuah_tg_reg, ARRAY_SIZE(gtpuah_tg_reg));
pr_info(MODULE_NAME": Unloading module\n");
}
module_init(gtpuah_tg_init);
module_exit(gtpuah_tg_exit);
MODULE_ALIAS("ipt6_GTPUAH");
MODULE_ALIAS("ipt_GTPUAH");
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