Linux Wimax filter

wimax_filter.c

/*
 * 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);