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