Created
October 29, 2011 07:47
-
-
Save neilxp/1324210 to your computer and use it in GitHub Desktop.
Linux Wimax filter
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* | |
* Wimax filter by Ricky Wen <cwen01390@cienet.com.cn> | |
*/ | |
#include <linux/types.h> | |
#include <linux/string.h> | |
#include <linux/fcntl.h> | |
#include <linux/if.h> | |
#include <linux/in.h> | |
#include <net/ip.h> | |
#include <net/tcp.h> | |
#include <net/udp.h> | |
#include <net/icmp.h> | |
#include <net/checksum.h> | |
#include <linux/if_ether.h> | |
#include <linux/netfilter.h> | |
#include <linux/netdevice.h> | |
#include <linux/skbuff.h> | |
#include <linux/etherdevice.h> | |
#include <linux/netfilter_ipv4.h> | |
MODULE_DESCRIPTION("WiMAX ASN message rilter"); | |
MODULE_LICENSE("GPL"); | |
MODULE_AUTHOR("Ricky Wen <cwen01390@cienet.com.cn"); | |
#define MODULE_NAME "WiMAX ASN filter" | |
/* parameters */ | |
/* Debug utilities */ | |
#define DEBUG 1 | |
/* print TRACE msgs */ | |
#ifdef DEBUG | |
#define TRACE \ | |
printk("%s: %s(): line %d\n", MODULE_NAME, __FUNCTION__, __LINE__) | |
#else | |
/* no TRACE */ | |
#define TRACE | |
#endif | |
/* Print kernel error */ | |
#define P_ERROR(args...) \ | |
printk(KERN_ERR MODULE_NAME ": " args) | |
/* Print kernel warning */ | |
#define P_WARN(args...) \ | |
printk(KERN_WARNING MODULE_NAME ": " args) | |
/* Print kernel notice */ | |
#define P_NOTICE(args...) \ | |
printk(KERN_NOTICE MODULE_NAME ": " args) | |
/* Print kernel info */ | |
#define P_INFO(args...) \ | |
printk(KERN_INFO MODULE_NAME ": " args) | |
/* Print verbose message. Enabled/disabled by 'log_level' param */ | |
#define P_VERBOSE(args...) \ | |
printk(MODULE_NAME ": " args) | |
/* Print debug message. Enabled/disabled by 'log_level' param */ | |
#ifdef DEBUG | |
#define P_DEBUG(args...) \ | |
printk("%s: %s(): ", MODULE_NAME, __FUNCTION__); \ | |
printk(args) | |
#else | |
#define P_DEBUG(args...) | |
#endif | |
struct wimaxhdr { | |
__u8 version; | |
__u8 flags; | |
__u8 ft; | |
__u8 opidmt; | |
__u16 len; | |
__u16 msid; | |
__u32 msid2; | |
__u32 rsv; | |
__u16 transid; | |
__u16 rsv2; | |
}; | |
#define MSINFO 103 | |
#define MSID 102 | |
struct typelen { | |
__u16 type; | |
__u16 len; | |
}; | |
char *match_msid(const char *tlv, int len) | |
{ | |
__u32 *idx = NULL; | |
int i = 0; | |
int scan_len = len / 4; | |
__u16 type = 0, length = 0; | |
struct typelen *tl = NULL; | |
char *ret = NULL; | |
idx = (__u32 *)tlv; | |
for (i = 0; i < scan_len; i++) | |
{ | |
if (i >= scan_len) | |
return NULL; | |
idx += i; | |
tl = (struct typelen *)idx; | |
if (tl == NULL) | |
return NULL; | |
type = ntohs(tl->type); | |
length = ntohs(tl->len); | |
if (type == MSID && length == 6) | |
{ | |
ret = (char *)(idx + 1); | |
break; | |
} | |
} | |
if (i < len) | |
{ | |
idx -= 1; | |
tl = (struct typelen *)idx; | |
if (tl == NULL) | |
return NULL; | |
type = ntohs(tl->type); | |
length = ntohs(tl->len); | |
if (type != MSINFO) | |
ret = NULL; | |
} | |
return ret; | |
} | |
static int is_context_req(int ft, int mt) | |
{ | |
return ((ft == 4 && mt == 2) ? 0 : -1); | |
} | |
static unsigned int wimax_filter(unsigned int hooknum, | |
struct sk_buff **skb, | |
const struct net_device *in, | |
const struct net_device *out, | |
int (*okfn)(struct sk_buff *)) | |
{ | |
struct sk_buff *pskb = NULL; | |
struct iphdr *iph = NULL; | |
struct udphdr *udph = NULL; | |
unsigned char *idx = NULL; | |
unsigned char *tlv = NULL; | |
int ft = 0, mt = 0; | |
unsigned short wlen = 0, dlen = 0; | |
int iplen = 0, udp_payload_len = 0; | |
__u32 srcip = 0, dstip = 0; | |
__u16 srcport = 0, dstport = 0; | |
struct wimaxhdr *whdr = NULL; | |
if (skb == NULL) | |
return NF_ACCEPT; | |
pskb = *skb; | |
if (pskb == NULL) | |
return NF_ACCEPT; | |
iph = pskb->nh.iph; | |
if (iph == NULL) | |
return NF_ACCEPT; | |
/* if not udp, pass it */ | |
if (iph->protocol != 0x11) | |
return NF_ACCEPT; | |
srcip = ntohl(iph->saddr); | |
dstip = ntohl(iph->daddr); | |
iplen = ntohs(iph->tot_len); | |
/* need only udp packet */ | |
udph = (struct udphdr *)((__u32 *)iph + iph->ihl); | |
if (udph == NULL) | |
return NF_ACCEPT; | |
srcport = ntohs(udph->source); | |
dstport = ntohs(udph->dest); | |
udp_payload_len = iplen - 20; | |
if (udp_payload_len < 20) | |
return NF_ACCEPT; | |
if (dstport != 2231) | |
return NF_ACCEPT; | |
P_DEBUG("==============================================\n"); | |
P_DEBUG("IP src: %d.%d.%d.%d\n",(srcip&0xff000000)>>24,(srcip&0xff0000)>>16,(srcip&0xff00)>>8,srcip&0xff); | |
P_DEBUG("IP dst: %d.%d.%d.%d\n",(dstip&0xff000000)>>24,(dstip&0xff0000)>>16,(dstip&0xff00)>>8,dstip&0xff); | |
P_DEBUG("Dest port: %u\n", dstport); | |
P_DEBUG("Src port: %u\n", srcport); | |
P_DEBUG("udp length: %d\n", udp_payload_len); | |
whdr = (struct wimaxhdr *)((__u32 *)udph + 2); | |
if (whdr == NULL) | |
return NF_ACCEPT; | |
P_DEBUG("WiMAX Version: %d\n", whdr->version); | |
/* validate wimax message, version 1 */ | |
if (whdr->version != 1) | |
return NF_ACCEPT; | |
/* function type */ | |
ft = whdr->ft; | |
/* op id and message type */ | |
mt = whdr->opidmt & 0x1f; | |
/* length */ | |
wlen = ntohs(whdr->len); | |
if (wlen < 20) | |
return NF_ACCEPT; | |
dlen = wlen - 20; | |
if (dlen == 0) | |
return NF_ACCEPT; | |
tlv = (char *)whdr + 20; | |
if (tlv == NULL) | |
return NF_ACCEPT; | |
/* idx to wimax payload */ | |
P_DEBUG("function type: %d, message type is %d\n", ft, mt); | |
P_DEBUG("wimax message length: %d, payload length: %d\n", wlen, dlen); | |
if (is_context_req(ft, mt)) | |
return NF_ACCEPT; /* not context req, return */ | |
idx = match_msid(tlv, dlen); | |
if (idx == NULL) | |
{ | |
P_DEBUG("Context req! But no ms id matched!\n"); | |
return NF_ACCEPT; | |
} | |
/* Now, we get the position of ms id in message context_req */ | |
P_DEBUG("**************\n"); | |
P_DEBUG("MS_ID: %02x:%02x:%02x:%02x:%02x:%02x\n", idx[0], idx[1], idx[2], idx[3], idx[4], idx[5]); | |
/* START TO CHANGE MSID */ | |
idx[0] = 0x43; idx[1] = 0x49; idx[2] = 0x65; idx[3] = 0x4e; idx[4] = 0x45; idx[5] = 0x54; | |
P_DEBUG("Modified MS_ID: %02x:%02x:%02x:%02x:%02x:%02x\n", idx[0], idx[1], idx[2], idx[3], idx[4], idx[5]); | |
P_DEBUG("Modified MS_ID: %c:%c:%c:%c:%c:%c\n", idx[0], idx[1], idx[2], idx[3], idx[4], idx[5]); | |
/* Compute checksums */ | |
//pskb->csum = 0; | |
// P_DEBUG("pskb->len: %u\n", pskb->len); | |
// P_DEBUG("pskb->csum: 0x%x\n", pskb->csum); | |
//pskb->csum = csum_partial((char *)whdr, udph->len - sizeof (struct udphdr), 0); | |
udph->check = 0; | |
//udph->check = csum_tcpudp_magic(iph->saddr, iph->daddr, | |
// udph->len, IPPROTO_UDP, | |
// csum_partial((char *)udph, sizeof(struct udphdr), pskb->csum)); | |
// ip_send_check(iph); | |
return NF_ACCEPT; | |
} | |
static struct nf_hook_ops wimax_filter_ops_out = { | |
.hook = wimax_filter, | |
.pf = PF_INET, | |
.hooknum = NF_IP_LOCAL_OUT, | |
.priority = NF_IP_PRI_FILTER - 100, | |
.owner = THIS_MODULE, | |
}; | |
static struct nf_hook_ops wimax_filter_ops_in = { | |
.hook = wimax_filter, | |
.pf = PF_INET, | |
.hooknum = NF_IP_LOCAL_IN, | |
.priority = NF_IP_PRI_FILTER - 100, | |
.owner = THIS_MODULE, | |
}; | |
int init_filter(void) | |
{ | |
int ret = 0; | |
ret = nf_register_hook(&wimax_filter_ops_in); | |
ret = nf_register_hook(&wimax_filter_ops_out); | |
return ret; | |
} | |
void cleanup_filter(void) | |
{ | |
nf_unregister_hook(&wimax_filter_ops_in); | |
nf_unregister_hook(&wimax_filter_ops_out); | |
} | |
static int __init init(void) | |
{ | |
return init_filter(); | |
} | |
static void __exit finish(void) | |
{ | |
cleanup_filter(); | |
} | |
module_init(init); | |
module_exit(finish); |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment