From aeb743eba07228437dd4fbb2a4372ade1d7af31a Mon Sep 17 00:00:00 2001
From: Lionel Gauthier <lionel.gauthier@eurecom.fr>
Date: Fri, 15 May 2015 15:29:28 +0000
Subject: [PATCH] socket no wait only for debugging
git-svn-id: http://svn.eurecom.fr/openair4G/trunk@7427 818b1a75-f10b-46b9-bf7c-635c3b92a50f
---
openair-cn/GTPV1-U/GTPUAH/xt_GTPUAH.c | 1540 +++++++++++++------------
1 file changed, 772 insertions(+), 768 deletions(-)
diff --git a/openair-cn/GTPV1-U/GTPUAH/xt_GTPUAH.c b/openair-cn/GTPV1-U/GTPUAH/xt_GTPUAH.c
index 5090369ded..9970222e35 100755
--- a/openair-cn/GTPV1-U/GTPUAH/xt_GTPUAH.c
+++ b/openair-cn/GTPV1-U/GTPUAH/xt_GTPUAH.c
@@ -1,768 +1,772 @@
-/*
- * 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*)>puh, 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*)>puh, 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");
+
--
GitLab