L3 layer portmap example

#include <asm/atomic.h>
#include <asm/checksum.h>
#include <asm/unaligned.h>
#include <linux/module.h>
#include <linux/init.h>
#include <linux/kernel.h>
#include <linux/net.h>
#include <linux/in.h>
#include <linux/types.h>
#include <linux/kthread.h>
#include <linux/wait.h>
#include <linux/skbuff.h>
#include <linux/string.h>
#include <linux/sysctl.h>
#include <linux/netfilter.h>
#include <linux/netfilter_ipv4.h>
#include <linux/ip.h>
#include <linux/workqueue.h>
#include <linux/jiffies.h>
#include <linux/route.h>
#include <linux/stddef.h>
#include <linux/mutex.h>
#include <linux/icmp.h>
#include <net/net_namespace.h>
#include <net/sock.h>
#include <net/route.h>
#include <net/inet_connection_sock.h>
#include <net/request_sock.h>
#include <net/icmp.h>
#include <net/ip.h>
#include <asm/byteorder.h>

#define err(msg) printk(KERN_ERR "%s failure.\n", msg)
#define SA struct sockaddr

struct connection_struct {
    __be32 caddr;
    __be32 vaddr;
    __be32 daddr;
    __be16 cport;
    __be16 vport;
    __be16 dport;
};

static struct connection_struct conn = {
    .caddr = 0xFA0AA8C0,      

    .vaddr = 0xBC0AA8C0,     
    .daddr = 0x130AA8C0,      
    .cport = 0,
    .vport = htons(8888),
    .dport = htons(7777),
};

static unsigned int sg_vs_in(unsigned int hooknum,
            struct sk_buff *skb,
            const struct net_device *in,
            const struct net_device *out,
            int (*okfn)(struct sk_buff *))
{
    struct rtable *rt;
    struct tcphdr *tcph;
    struct iphdr *iph = ip_hdr(skb);
    if(iph == NULL){
        err("ip_hdr");
        return NF_DROP;
    }

    tcph = (struct tcphdr *)(skb_network_header(skb) + ip_hdrlen(skb));
    if (tcph == NULL) {
        err("tcph is NULL.");
        return NF_DROP;             
    }                     

    if (iph->saddr == conn.caddr && tcph->dest == conn.vport) {        
    
        conn.cport = tcph->source;

        tcph->dest = conn.dport;     
        iph->saddr = conn.vaddr;     
        iph->daddr = conn.daddr;     

        tcph->check = 0;            /* tcp_hdrlen(skb) get L4 header length */
        skb->csum = skb_checksum(skb, ip_hdrlen(skb), skb->len - ip_hdrlen(skb), 0);
        tcph->check = csum_tcpudp_magic(iph->saddr, iph->daddr, skb->len - ip_hdrlen(skb), iph->protocol, skb->csum);

        ip_send_check(ip_hdr(skb));

        struct flowi fl = {        /* struct flowi: is route search key */
            .oif = 0,        /* output device identifier */
            .nl_u = {
                .ip4_u = {
                    .daddr = iph->daddr,                /* real server ip */
                    .saddr = 0,
                    .tos = RT_TOS(iph->tos), } },
        };


        if (ip_route_output_key(&init_net, &rt, &fl) != 0) {
            err("ip_route_output_key");
            return NF_DROP;
        }

        skb_dst_drop(skb);
        skb_dst_set(skb, &rt->u.dst);    
        skb->local_df = 1;

        dst_output(skb);

        return NF_STOLEN;

    } else if (iph->saddr == conn.daddr && tcph->source == conn.dport) {        /* dest --> virtual */
        tcph->source = conn.vport;
        tcph->dest = conn.cport;
        iph->saddr = conn.vaddr;
        iph->daddr = conn.caddr;

        tcph->check = 0;
        skb->csum = skb_checksum(skb, ip_hdrlen(skb), skb->len - ip_hdrlen(skb), 0);
        tcph->check = csum_tcpudp_magic(iph->saddr, iph->daddr, skb->len - ip_hdrlen(skb), iph->protocol, skb->csum);

        ip_send_check(ip_hdr(skb));

        struct flowi fl = {
            .oif = 0,    
            .nl_u = {
                .ip4_u = {
                    .daddr = iph->daddr,                
                    .saddr = 0,
                    .tos = RT_TOS(iph->tos), } },
        };

        if (ip_route_output_key(&init_net, &rt, &fl) != 0) {
            err("ip_route_output_key");
            return NF_DROP;
        }

        skb_dst_drop(skb);
        skb_dst_set(skb, &rt->u.dst);    
        skb->local_df = 1;

        dst_output(skb);

        return NF_STOLEN;

    }

    return NF_ACCEPT;
}

static struct nf_hook_ops sg_nf_ops = {
    .hook        = sg_vs_in,    
    .owner        = THIS_MODULE,
    .pf        = PF_INET,
    .hooknum = NF_INET_LOCAL_IN,        
};

static int main_init(void)
{
    int rc;

    rc = nf_register_hook(&sg_nf_ops);
    if (rc < 0) {
        err("nf_register_hook");
        goto out;
    }

    return 0;
out:
    return -1;
}

static void main_exit(void)
{
    nf_unregister_hook(&sg_nf_ops);
}

module_init(main_init);
module_exit(main_exit);
MODULE_LICENSE("GPL");