Add DNAT/SNAT support

JIRA: VSPERF-72

This change adds optional IPv4 DNAT support to l2fwd module for level two
forwarding. With this change the l2fwd module can be used for testing
complex IP configuration routing with virtual switches.

Other L2 forwarding modules provide only Level 2 pass through. These can be
used for complex IP chain processing but rely on port forwarding in the switch.
With this module each packet which is forwarded optionally gets a new
destination IP and source masquerade. So a test packet can be routed to eth1
and forwarded on eth2 with a different target IP and source address.

To use this module compile it for your kernel and use the 'insmod' command
to insert it.

With no arguments this will forward eth1 to eth2 without modification.

With 'net1=ethX net2=ethX' Level 2 forwarding can be done between arbitrary
ports.

With 'netX=ethX,XX.XX.XX.XX,xx:xx:xx:xx:xx:xx' the packets which are forwarded
on the target port will be given the new destination IP and mac address. One
or both ports may enable DNAT.

Change-Id: If24281a6841930a7a85e4536da96b980ed48df1b
Signed-off-by: Gene Snider <eugene.snider@huawei.com>
Reviewed-by: Billy O Mahony <billy.o.mahony@intel.com>

1 files changed, 218 insertions, 10 deletions

diff --git a/src/l2fwd/l2fwd.c b/src/l2fwd/l2fwd.c
index 9f2ec314..338b6f50 100644
--- a/src/l2fwd/l2fwd.c
+++ b/src/l2fwd/l2fwd.c
@@ -38,20 +38,28 @@
 #include <linux/rtnetlink.h>
 #include <linux/prefetch.h>
 #include <linux/log2.h>
+#include <linux/gfp.h>
+#include <linux/slab.h>
+
+#include <linux/ip.h>
+#include <linux/in.h>
+#include <linux/icmp.h>
+#include <linux/inetdevice.h>
+
 
 static char *net1 = "eth1";
 module_param(net1, charp, 0);
-MODULE_PARM_DESC(net1, "The first net device name (default is eth1)");
+MODULE_PARM_DESC(net1, "The first net device name and optional DNAT parameters (default is eth1)");
 
 static char *net2 = "eth2";
 module_param(net2, charp, 0);
-MODULE_PARM_DESC(net2, "The second net device name (default is eth2)");
+MODULE_PARM_DESC(net2, "The second net device name and optional DNAT parameters (default is eth2)");
 
 static bool print = false;
 module_param(print, bool, 0);
 MODULE_PARM_DESC(print, "Log forwarding statistics (default is false)");
 
-static int stats_interval = 10000;
+static int stats_interval = 10;
 module_param(stats_interval, int, 0);
 MODULE_PARM_DESC(print, "Forwarding statistics packet interval (default is 10000)");
 
@@ -62,10 +70,31 @@ MODULE_PARM_DESC(terminate, "Free skb instead of forwarding");
 static struct net_device *dev1, *dev2;
 int count;
 
+static struct
+{
+    uint eth1 : 1;
+    uint eth2 : 1;
+} dnat_enabled;
+
+static uint octet0[4];
+static uint mac0[6];
+static u8 s_octet0[4];
+static u8 s_mac0[6];
+
+static uint octet1[4];
+static uint mac1[6];
+static u8 s_octet1[4];
+static u8 s_mac1[6];
 static rx_handler_result_t netdev_frame_hook(struct sk_buff **pskb)
 {
     struct sk_buff *skb = *pskb;
     struct net_device *dev;
+    char *data = skb->data;
+    struct ethhdr *eh = (struct ethhdr *)skb_mac_header(skb);
+    struct iphdr *ih = (struct iphdr *)skb_network_header(skb);
+    struct icmphdr *icmph = icmp_hdr(skb);
+
+    rx_handler_result_t retval = RX_HANDLER_CONSUMED;
 
     if (unlikely(skb->pkt_type == PACKET_LOOPBACK))
         return RX_HANDLER_PASS;
@@ -81,33 +110,211 @@ static rx_handler_result_t netdev_frame_hook(struct sk_buff **pskb)
         }
     else
         {
+
+            u32 *daddr = &(ih->daddr);
+            u32 *saddr = &(ih->saddr);
+            unsigned short proto = ntohs(eh->h_proto);
+
+            uint use_dnat = dev == dev1 ? dnat_enabled.eth1:dnat_enabled.eth2;
+
+            if (unlikely(proto != ETH_P_IP ))
+                return RX_HANDLER_PASS;
+
+#ifdef DEBUG
+            printk("use_dnat %d proto %x ETH_P_IP %x Dest MAC - IP %d.%d.%d.%d MAC %x:%x:%x:%x:%x:%x\n",use_dnat,proto,ETH_P_IP,(u8)daddr[0],(u8)daddr[1],(u8)daddr[2],(u8)daddr[3],eh->h_dest[0],eh->h_dest[1],eh->h_dest[2],eh->h_dest[3],eh->h_dest[4],eh->h_dest[5]);
+#endif
+            if ( (use_dnat == 1) && (proto == ETH_P_IP) )
+                {
+                    unsigned int *t_addr = dev == dev1 ? &octet0[0]:&octet1[0];
+                    u8 *s_addr = dev == dev1 ? &s_octet0[0]:&s_octet1[0];
+
+
+                    ((u8 *)daddr)[0] = (u8)t_addr[0];
+                    ((u8 *)daddr)[1] = (u8)t_addr[1];
+                    ((u8 *)daddr)[2] = (u8)t_addr[2];
+                    ((u8 *)daddr)[3] = (u8)t_addr[3];
+
+                    t_addr = dev == dev1 ? &mac0[0]:&mac1[0];
+
+                    eh->h_dest[0] = (unsigned char)t_addr[0];
+                    eh->h_dest[1] = (unsigned char)t_addr[1];
+                    eh->h_dest[2] = (unsigned char)t_addr[2];
+                    eh->h_dest[3] = (unsigned char)t_addr[3];
+                    eh->h_dest[4] = (unsigned char)t_addr[4];
+                    eh->h_dest[5] = (unsigned char)t_addr[5];
+
+#ifdef DEBUG
+                    printk("After DNAT: Dest MAC - IP %d.%d.%d.%d MAC %x:%x:%x:%x:%x:%x\n",daddr[0],daddr[1],daddr[2],daddr[3],eh->h_dest[0],eh->h_dest[1],eh->h_dest[2],eh->h_dest[3],eh->h_dest[4],eh->h_dest[5]);
+#endif
+
+                    eh->h_source[0] = (unsigned char)dev->dev_addr[0];
+                    eh->h_source[1] = (unsigned char)dev->dev_addr[1];
+                    eh->h_source[2] = (unsigned char)dev->dev_addr[2];
+                    eh->h_source[3] = (unsigned char)dev->dev_addr[3];
+                    eh->h_source[4] = (unsigned char)dev->dev_addr[4];
+                    eh->h_source[5] = (unsigned char)dev->dev_addr[5];
+
+                    ((u8 *)saddr)[0] = s_addr[0];
+                    ((u8 *)saddr)[1] = s_addr[1];
+                    ((u8 *)saddr)[2] = s_addr[2];
+                    ((u8 *)saddr)[3] = s_addr[3];
+
+                    skb->ip_summed = CHECKSUM_COMPLETE;
+                    skb->csum = skb_checksum_complete(skb);
+                    ih->check = 0;
+                    ih->check = ip_fast_csum((unsigned char *)ih, ih->ihl);
+
+#ifdef DEBUG
+                    printk("After DNAT: Source MAC - IP %d.%d.%d.%d MAC %u:%u:%u:%u:%u:%u\n",saddr[0],saddr[1],saddr[2],saddr[3],eh->h_source[0],eh->h_source[1],eh->h_source[2],eh->h_source[3],eh->h_source[4],eh->h_source[5]);
+#endif
+                }
+
             skb->dev = dev;
             skb_push(skb, ETH_HLEN);
             dev_queue_xmit(skb);
         }
 
-    return RX_HANDLER_CONSUMED;
+    return retval;
 }
 
 static int __init l2fwd_init_module(void)
 {
     int err = -1;
+    char *t_ptr;
+    int fCount;
+    char *dnat_fmt_suffix = " %3d.%3d.%3d.%3d %2x:%2x:%2x:%2x:%2x:%2x";
+    char *dnat_fmt;
+    char name_fmt_str[IFNAMSIZ+1];
+    char t_name[IFNAMSIZ+1];
+
+
+
+    sprintf(name_fmt_str,"%%%ds",IFNAMSIZ);
+    dnat_fmt = (char *)kmalloc(strlen(name_fmt_str)+strlen(dnat_fmt_suffix)+1,GFP_KERNEL);
+    sprintf(dnat_fmt,"%s%s",name_fmt_str,dnat_fmt_suffix);
+
+    t_ptr=net1;
+    while(*t_ptr != '\0') *t_ptr == ',' ? *t_ptr++ = ' ':*t_ptr++;
+
+    fCount = sscanf(net1,dnat_fmt,t_name,&octet0[0],&octet0[1],&octet0[2],
+                                         &octet0[3],&mac0[0],&mac0[1],
+                                         &mac0[2],&mac0[3],&mac0[4],&mac0[5]);
 
-    dev1 = dev_get_by_name(&init_net, net1);
+    dev1 = dev_get_by_name(&init_net, t_name);
     if (!dev1)
         {
-            printk("can't get device %s\n", net1);
+            printk("can't get device %s\n", t_name);
             err = -ENODEV;
             goto out;
         }
+    if (fCount >1 && fCount <11 )
+        {
+            printk("Both DNAT IP and Mac Address must be provided\n");
+            err = -EINVAL;
+            goto out;
+        }
+    else if (fCount==11 )
+        {
+            struct in_device *in_dev = rcu_dereference(dev1->ip_ptr);
+            __be32 ifaddr = 0;
+            struct in_ifaddr *ifap;
+
+            dnat_enabled.eth1 = 1;
+            // in_dev has a list of IP addresses (because an interface can have multiple)
+            for (ifap = in_dev->ifa_list; ifap != NULL;
+                    ifap = ifap->ifa_next)
+                {
+                    ifaddr = ifap->ifa_address; // is the IPv4 address
+                }
+            if (ifaddr == 0)
+                {
+                    printk("%s interface ip address list is null!\n",t_name);
+                    err = -ENODEV;
+                    goto out;
+                }
+
+            s_octet0[0] = ((u8 *)&ifaddr)[0];
+            s_octet0[1] = ((u8 *)&ifaddr)[1];
+            s_octet0[2] = ((u8 *)&ifaddr)[2];
+            s_octet0[3] = ((u8 *)&ifaddr)[3];
+
+            s_mac0[0] = dev1->dev_addr[0];
+            s_mac0[1] = dev1->dev_addr[1];
+            s_mac0[2] = dev1->dev_addr[2];
+            s_mac0[3] = dev1->dev_addr[3];
+            s_mac0[4] = dev1->dev_addr[4];
+            s_mac0[5] = dev1->dev_addr[5];
+
+#ifdef DEBUG
+            printk("DNAT Enabled for %s IP %d.%d.%d.%d MAC %x:%x:%x:%x:%x:%x\n",t_name,octet0[0],octet0[1],octet0[2],octet0[3],mac0[0],mac0[1],mac0[2],mac0[3],mac0[4],mac0[5]);
+            printk("SNAT IP %d.%d.%d.%d SNAT MAC  %x:%x:%x:%x:%x:%x\n",s_octet0[0],s_octet0[1],s_octet0[2],s_octet0[3],s_mac0[0],s_mac0[1],s_mac0[2],s_mac0[3],s_mac0[4],mac0[5]);
+        }
+    else if (fCount==1 )
+        {
+            printk("Pure Level 2 forward enabled for %s\n",t_name);
+#endif
+        }
+
+
+    t_ptr=net2;
+    while(*t_ptr != '\0') *t_ptr == ',' ? *t_ptr++ = ' ':*t_ptr++;
 
-    dev2 = dev_get_by_name(&init_net, net2);
+    fCount = sscanf(net2,dnat_fmt,t_name,&octet1[0],&octet1[1],&octet1[2],&octet1[3],&mac1[0],&mac1[1],&mac1[2],&mac1[3],&mac1[4],&mac1[5]);
+
+    dev2 = dev_get_by_name(&init_net, t_name);
     if (!dev2)
         {
-            printk("can't get device %s\n", net2);
+            printk("can't get device %s\n", t_name);
             err = -ENODEV;
             goto out_dev1;
         }
+    if (fCount >1 && fCount <11 )
+        {
+            printk("Both DNAT IP and Mac Address must be provided\n");
+            err = -EINVAL;
+            goto out;
+        }
+    else if (fCount==11 )
+        {
+            struct in_device *in_dev = rcu_dereference(dev2->ip_ptr);
+            __be32 ifaddr = 0;
+            struct in_ifaddr *ifap;
+            // dev has a list of IP addresses (because an interface can have multiple)
+            dnat_enabled.eth2 = 1;
+            for (ifap = in_dev->ifa_list; ifap != NULL;
+                    ifap = ifap->ifa_next)
+                {
+                    ifaddr = ifap->ifa_address; // is the IPv4 address
+                }
+            if (ifaddr == 0)
+                {
+                    printk("%s interface ip address list is null!\n",t_name);
+                    err = -ENODEV;
+                    goto out;
+                }
+            /* ifa_local: ifa_address is the remote point in ppp */
+
+            s_octet1[0] = ((u8 *)&ifaddr)[0];
+            s_octet1[1] = ((u8 *)&ifaddr)[1];
+            s_octet1[2] = ((u8 *)&ifaddr)[2];
+            s_octet1[3] = ((u8 *)&ifaddr)[3];
+
+            s_mac1[0] = dev2->dev_addr[0];
+            s_mac1[1] = dev2->dev_addr[1];
+            s_mac1[2] = dev2->dev_addr[2];
+            s_mac1[3] = dev2->dev_addr[3];
+            s_mac1[4] = dev2->dev_addr[4];
+            s_mac1[5] = dev2->dev_addr[5];
+
+#ifdef NEVER
+            printk("DNAT Enabled for %s IP %d.%d.%d.%d MAC %x:%x:%x:%x:%x:%x\n",t_name,octet1[0],octet1[1],octet1[2],octet1[3],mac1[0],mac1[1],mac1[2],mac1[3],mac1[4],mac1[5]);
+            printk("SNAT IP %d.%d.%d.%d SNAT MAC  %x:%x:%x:%x:%x:%x\n",s_octet1[0],s_octet1[1],s_octet1[2],s_octet1[3],s_mac1[0],s_mac1[1],s_mac1[2],s_mac1[3],s_mac1[4],mac1[5]);
+        }
+    else if (fCount==1 )
+        {
+            printk("Level 2 forward enabled for %s\n",t_name);
+#endif
+        }
 
     rtnl_lock();
     err = netdev_rx_handler_register(dev1, netdev_frame_hook, dev2);
@@ -125,6 +332,7 @@ static int __init l2fwd_init_module(void)
             netdev_rx_handler_unregister(dev1);
             goto out_dev2;
         }
+
     dev_set_promiscuity(dev2, 1);
 
     rtnl_unlock();
@@ -162,6 +370,6 @@ static void __exit l2fwd_exit_module(void)
 module_init(l2fwd_init_module);
 module_exit(l2fwd_exit_module);
 
-MODULE_DESCRIPTION("Huawei L2 Forwarding module");
+MODULE_DESCRIPTION("Huawei L2 Forwarding module with DNAT");
 MODULE_LICENSE("GPL");
-MODULE_VERSION("0.1");
+MODULE_VERSION("1.0");