Andrei

/*
 *      Author: andrei.sambra@telecom-sudparis.eu
 *      GPLv3 License applies to this code.
 *
 * */

#include <linux/module.h>       /* Needed by all modules */
#include <linux/kernel.h>       /* Needed for KERN_INFO */
#include <linux/init.h>         /* Needed for the macros */
#include <linux/skbuff.h>
#include <linux/netfilter.h>
#include <linux/netfilter_ipv4.h>
#include <linux/ip.h>
#include <linux/udp.h>
#include "rpmp.h"               /* Needed for our structures */


#define DEBUG 0

struct sk_buff *sock_buff;
struct iphdr *ip_header;
struct udphdr *udp_header;
struct rpmphdr *rpmp_header;
static struct nf_hook_ops nfho;

static unsigned int hook_func(unsigned int hooknum,
                        struct sk_buff *skb,
                                const struct net_device *in,
                                const struct net_device *out,
                                int (*okfn)(struct sk_buff *))
{
        sock_buff = skb;

        if (!sock_buff) {
                return NF_ACCEPT;
        } else {
                ip_header = (struct iphdr *)skb_network_header(sock_buff);
                if (!ip_header) {
                        return NF_ACCEPT;
                } else {
                        if (ip_header->protocol == IPPROTO_RPMP) {

                                rpmp_header = (struct rpmphdr *)(skb_transport_header(sock_buff)+sizeof(struct iphdr));
#if DEBUG > 0
                                printk(KERN_INFO "[RPMP] DEBUG: th: 0p%p\n", rpmp_header);
                                printk(KERN_INFO "[RPMP] DEBUG: nh: 0p%p\n", skb_network_header(sock_buff));
                                printk(KERN_INFO "[RPMP] DEBUG: mh: 0p%p\n", skb_mac_header(sock_buff));
                                printk(KERN_INFO "[RPMP] DEBUG: Length: rpmp_header=%d | dport=%d | type=%d.\n",
                                                sizeof(rpmp_header),
                                                sizeof(rpmp_header->dport),
                                                sizeof(rpmp_header->type));
                                printk(KERN_INFO "[RPMP] DEBUG: From IP address: %d.%d.%d.%dn",
                                          ip_header-saddr & 0x000000FF,
                                          (ip_header->saddr & 0x0000FF00) >> 8,
                                          (ip_header->saddr & 0x00FF0000) >> 16,
                                          (ip_header->saddr & 0xFF000000) >> 24);

#endif
                                printk(KERN_INFO "[RPMP] Got a RPMP packet for port=%d (type:%d).\n",
                                                ntohs(rpmp_header->dport),
                                                ntohs(rpmp_header->type));

                                /* Callback function here*/

                                return NF_DROP;
                        } else {
                                return NF_ACCEPT;
                        }
                }
        }
}

static int __init init_main(void)
{
        nfho.hook     = hook_func;
        nfho.hooknum  = 1;
        nfho.pf       = PF_INET;
        nfho.priority = NF_IP_PRI_FIRST;
        nf_register_hook(&nfho);

#if DEBUG > 0
        printk(KERN_INFO "[RPMP] Successfully inserted protocol module into kernel.\n");
#endif

        return 0;
}

static void __exit cleanup_main(void)
{
        nf_unregister_hook(&nfho);

#if DEBUG > 0
        printk(KERN_INFO "[RPMP] Successfully unloaded protocol module.\n");
#endif
}

module_init(init_main);
module_exit(cleanup_main);

/*
 *      Declaring code as GPL.
 */
MODULE_LICENSE("GPLv3");
MODULE_AUTHOR(DRIVER_AUTHOR);           /* Who wrote this module? */
MODULE_DESCRIPTION(DRIVER_DESC);        /* What does this module do */