/* * GTPu klm for Linux/iptables * * Copyright (c) 2010-2011 Polaris Networks * Author: Pradip Biswas * * 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 #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include // CONNMARK #include #include #include #include #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 #define TRACE_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 "); 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; #if defined(FLAG_GTPV1U_KERNEL_THREAD_SOCK_NO_WAIT) int success_read = 0; int failed_read = 0; #endif 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) { if (size != -EAGAIN) { pr_info(MODULE_NAME": error getting datagram, sock_recvmsg error = %d\n", size); } #if defined(FLAG_GTPV1U_KERNEL_THREAD_SOCK_NO_WAIT) success_read = 0; failed_read += 1; if (failed_read > 10) failed_read = 10; usleep_range(failed_read*20,failed_read*200); #endif } else { #if defined(FLAG_GTPV1U_KERNEL_THREAD_SOCK_NO_WAIT) success_read += 1; failed_read = 0; #endif 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(FLAG_GTPV1U_KERNEL_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 targinfo ltun %u to skb_pP mark %u\n", ((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->ltun, old_skb_pP->mark); newmark = ((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->ltun; } 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); if (newmark != ((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->ltun) { pr_warn(MODULE_NAME": _gtpuah_target_add restore mark 0x%x mismatch ltun 0x%x (rtun 0x%x)", newmark, ((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->ltun, ((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->rtun); } } /* Add GTPu header */ gtpuh.flags = 0x30; /* v1 and Protocol-type=GTP */ gtpuh.msgtype = 0xff; /* T-PDU */ gtpuh.length = htons(orig_iplen); gtpuh.tunid = htonl(((const struct xt_gtpuah_target_info *)(par_pP->targinfo))->rtun); _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");