src/l2fwd/l2fwd.c

   1 /*
   2 * Copyright 2015 Futurewei Inc.
   3 *
   4 * l2fwd is free software: you can redistribute it and/or modify
   5 * it under the terms of version 2 the GNU General Public License as published
   6 * by the Free Software Foundation only
   7
   8 * l2fwd is distributed in the hope that it will be useful,
   9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11 * GNU General Public License for more details.
  12
  13 * You should have received a copy of the GNU General Public License
  14 * along with this code.  If not, see
  15 * https://www.gnu.org/licenses/gpl-2.0.html
  16 *
  17 * Unless required by applicable law or agreed to in writing, software
  18 * distributed under the License is distributed on an "AS IS" BASIS,
  19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  20 * See the License for the specific language governing permissions and
  21 * limitations under the License.
  22 */
  23
  24
  25 #include <linux/module.h>
  26 #include <linux/kernel.h>
  27 #include <linux/errno.h>
  28 #include <linux/string.h>
  29 #include <linux/init.h>
  30 #include <linux/interrupt.h>
  31 #include <linux/pci.h>
  32 #include <linux/aer.h>
  33 #include <linux/netdevice.h>
  34 #include <linux/etherdevice.h>
  35 #include <linux/if.h>
  36 #include <linux/if_ether.h>
  37 #include <linux/in.h>
  38 #include <linux/rtnetlink.h>
  39 #include <linux/prefetch.h>
  40 #include <linux/log2.h>
  41 #include <linux/gfp.h>
  42 #include <linux/slab.h>
  43 #include <linux/version.h>
  44
  45 #include <linux/ip.h>
  46 #include <linux/in.h>
  47 #include <linux/icmp.h>
  48 #include <linux/inetdevice.h>
  49
  50
  51 static char *net1 = "eth1";
  52 module_param(net1, charp, 0);
  53 MODULE_PARM_DESC(net1, "The first net device name and optional DNAT parameters (default is eth1)");
  54
  55 static char *net2 = "eth2";
  56 module_param(net2, charp, 0);
  57 MODULE_PARM_DESC(net2, "The second net device name and optional DNAT parameters (default is eth2)");
  58
  59 static bool print = false;
  60 module_param(print, bool, 0);
  61 MODULE_PARM_DESC(print, "Log forwarding statistics (default is false)");
  62
  63 static int stats_interval = 10;
  64 module_param(stats_interval, int, 0);
  65 MODULE_PARM_DESC(print, "Forwarding statistics packet interval (default is 10000)");
  66
  67 static bool terminate = false;
  68 module_param(terminate, bool, 0);
  69 MODULE_PARM_DESC(terminate, "Free skb instead of forwarding");
  70
  71 #if LINUX_VERSION_CODE >= KERNEL_VERSION(3,18,0)
  72 #define BURST_MODE
  73 static short burst = 1;
  74 module_param(burst, short, 0);
  75 MODULE_PARM_DESC(burst, "Send burst-many packets to output device at once (default is 1)");
  76
  77 short burst_count;
  78 #endif
  79 static struct net_device *dev1, *dev2;
  80 int count;
  81
  82 static struct
  83 {
  84     uint eth1 : 1;
  85     uint eth2 : 1;
  86 } dnat_enabled;
  87
  88 static uint octet0[4];
  89 static uint mac0[6];
  90 static u8 s_octet0[4];
  91 static u8 s_mac0[6];
  92
  93 static uint octet1[4];
  94 static uint mac1[6];
  95 static u8 s_octet1[4];
  96 static u8 s_mac1[6];
  97 static rx_handler_result_t netdev_frame_hook(struct sk_buff **pskb)
  98 {
  99     struct sk_buff *skb = *pskb;
 100     struct net_device *dev;
 101     char *data = skb->data;
 102     struct ethhdr *eh = (struct ethhdr *)skb_mac_header(skb);
 103     struct iphdr *ih = (struct iphdr *)skb_network_header(skb);
 104     struct icmphdr *icmph = icmp_hdr(skb);
 105
 106     rx_handler_result_t retval = RX_HANDLER_CONSUMED;
 107
 108     if (unlikely(skb->pkt_type == PACKET_LOOPBACK))
 109         return RX_HANDLER_PASS;
 110
 111     dev = (struct net_device*) rcu_dereference_rtnl(skb->dev->rx_handler_data);
 112     count++;
 113     if ( ((count % stats_interval) == 0) && print )
 114         printk("l2fwd count %d\n", count);
 115
 116     if (terminate)
 117         {
 118             kfree_skb(skb);
 119         }
 120     else
 121         {
 122
 123             u32 *daddr = &(ih->daddr);
 124             u32 *saddr = &(ih->saddr);
 125             unsigned short proto = ntohs(eh->h_proto);
 126
 127             uint use_dnat = dev == dev1 ? dnat_enabled.eth1:dnat_enabled.eth2;
 128
 129             if (unlikely(proto != ETH_P_IP ))
 130                 return RX_HANDLER_PASS;
 131
 132 #ifdef DEBUG
 133             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]);
 134 #endif
 135             if ( (use_dnat == 1) && (proto == ETH_P_IP) )
 136                 {
 137                     unsigned int *t_addr = dev == dev1 ? &octet0[0]:&octet1[0];
 138                     u8 *s_addr = dev == dev1 ? &s_octet0[0]:&s_octet1[0];
 139
 140
 141                     ((u8 *)daddr)[0] = (u8)t_addr[0];
 142                     ((u8 *)daddr)[1] = (u8)t_addr[1];
 143                     ((u8 *)daddr)[2] = (u8)t_addr[2];
 144                     ((u8 *)daddr)[3] = (u8)t_addr[3];
 145
 146                     t_addr = dev == dev1 ? &mac0[0]:&mac1[0];
 147
 148                     eh->h_dest[0] = (unsigned char)t_addr[0];
 149                     eh->h_dest[1] = (unsigned char)t_addr[1];
 150                     eh->h_dest[2] = (unsigned char)t_addr[2];
 151                     eh->h_dest[3] = (unsigned char)t_addr[3];
 152                     eh->h_dest[4] = (unsigned char)t_addr[4];
 153                     eh->h_dest[5] = (unsigned char)t_addr[5];
 154
 155 #ifdef DEBUG
 156                     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]);
 157 #endif
 158
 159                     eh->h_source[0] = (unsigned char)dev->dev_addr[0];
 160                     eh->h_source[1] = (unsigned char)dev->dev_addr[1];
 161                     eh->h_source[2] = (unsigned char)dev->dev_addr[2];
 162                     eh->h_source[3] = (unsigned char)dev->dev_addr[3];
 163                     eh->h_source[4] = (unsigned char)dev->dev_addr[4];
 164                     eh->h_source[5] = (unsigned char)dev->dev_addr[5];
 165
 166                     ((u8 *)saddr)[0] = s_addr[0];
 167                     ((u8 *)saddr)[1] = s_addr[1];
 168                     ((u8 *)saddr)[2] = s_addr[2];
 169                     ((u8 *)saddr)[3] = s_addr[3];
 170
 171                     skb->ip_summed = CHECKSUM_COMPLETE;
 172                     skb->csum = skb_checksum_complete(skb);
 173                     ih->check = 0;
 174                     ih->check = ip_fast_csum((unsigned char *)ih, ih->ihl);
 175
 176 #ifdef DEBUG
 177                     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]);
 178 #endif
 179                 }
 180
 181             skb->dev = dev;
 182             skb_push(skb, ETH_HLEN);
 183 #ifdef BURST_MODE
 184             if (burst > 1)
 185                 {
 186                    struct netdev_queue *txq = netdev_get_tx_queue(dev, 0);
 187                    skb_set_queue_mapping(skb, 0);
 188
 189                    if (!netif_xmit_frozen_or_stopped(txq))
 190                        {
 191                            const struct net_device_ops *ops = dev->netdev_ops;
 192                            int status = NETDEV_TX_OK;
 193                            skb->xmit_more = --burst_count > 0 ? 1 : 0;
 194                            status = ops->ndo_start_xmit(skb, dev);
 195                            if (status == NETDEV_TX_OK)
 196                                txq_trans_update(txq);
 197                            if (!burst_count)
 198                                burst_count = burst;
 199                        }
 200
 201                 }
 202             else
 203                 {
 204                     dev_queue_xmit(skb);
 205                 }
 206 #else
 207             dev_queue_xmit(skb);
 208 #endif
 209         }
 210
 211     return retval;
 212 }
 213
 214 static int __init l2fwd_init_module(void)
 215 {
 216     int err = -1;
 217     char *t_ptr;
 218     int fCount;
 219     char *dnat_fmt_suffix = " %3d.%3d.%3d.%3d %2x:%2x:%2x:%2x:%2x:%2x";
 220     char *dnat_fmt;
 221     char name_fmt_str[IFNAMSIZ+1];
 222     char t_name[IFNAMSIZ+1];
 223
 224 #ifdef BURST_MODE
 225     burst_count = burst;
 226 #endif
 227
 228     sprintf(name_fmt_str,"%%%ds",IFNAMSIZ);
 229     dnat_fmt = (char *)kmalloc(strlen(name_fmt_str)+strlen(dnat_fmt_suffix)+1,GFP_KERNEL);
 230     sprintf(dnat_fmt,"%s%s",name_fmt_str,dnat_fmt_suffix);
 231
 232     t_ptr=net1;
 233     while(*t_ptr != '\0') *t_ptr == ',' ? *t_ptr++ = ' ':*t_ptr++;
 234
 235     fCount = sscanf(net1,dnat_fmt,t_name,&octet0[0],&octet0[1],&octet0[2],
 236                                          &octet0[3],&mac0[0],&mac0[1],
 237                                          &mac0[2],&mac0[3],&mac0[4],&mac0[5]);
 238
 239     dev1 = dev_get_by_name(&init_net, t_name);
 240     if (!dev1)
 241         {
 242             printk("can't get device %s\n", t_name);
 243             err = -ENODEV;
 244             goto out;
 245         }
 246     if (fCount >1 && fCount <11 )
 247         {
 248             printk("Both DNAT IP and Mac Address must be provided\n");
 249             err = -EINVAL;
 250             goto out;
 251         }
 252     else if (fCount==11 )
 253         {
 254             struct in_device *in_dev = rcu_dereference(dev1->ip_ptr);
 255             __be32 ifaddr = 0;
 256             struct in_ifaddr *ifap;
 257
 258             dnat_enabled.eth1 = 1;
 259             // in_dev has a list of IP addresses (because an interface can have multiple)
 260             for (ifap = in_dev->ifa_list; ifap != NULL;
 261                     ifap = ifap->ifa_next)
 262                 {
 263                     ifaddr = ifap->ifa_address; // is the IPv4 address
 264                 }
 265             if (ifaddr == 0)
 266                 {
 267                     printk("%s interface ip address list is null!\n",t_name);
 268                     err = -ENODEV;
 269                     goto out;
 270                 }
 271
 272             s_octet0[0] = ((u8 *)&ifaddr)[0];
 273             s_octet0[1] = ((u8 *)&ifaddr)[1];
 274             s_octet0[2] = ((u8 *)&ifaddr)[2];
 275             s_octet0[3] = ((u8 *)&ifaddr)[3];
 276
 277             s_mac0[0] = dev1->dev_addr[0];
 278             s_mac0[1] = dev1->dev_addr[1];
 279             s_mac0[2] = dev1->dev_addr[2];
 280             s_mac0[3] = dev1->dev_addr[3];
 281             s_mac0[4] = dev1->dev_addr[4];
 282             s_mac0[5] = dev1->dev_addr[5];
 283
 284 #ifdef DEBUG
 285             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]);
 286             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]);
 287         }
 288     else if (fCount==1 )
 289         {
 290             printk("Pure Level 2 forward enabled for %s\n",t_name);
 291 #endif
 292         }
 293
 294
 295     t_ptr=net2;
 296     while(*t_ptr != '\0') *t_ptr == ',' ? *t_ptr++ = ' ':*t_ptr++;
 297
 298     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]);
 299
 300     dev2 = dev_get_by_name(&init_net, t_name);
 301     if (!dev2)
 302         {
 303             printk("can't get device %s\n", t_name);
 304             err = -ENODEV;
 305             goto out_dev1;
 306         }
 307     if (fCount >1 && fCount <11 )
 308         {
 309             printk("Both DNAT IP and Mac Address must be provided\n");
 310             err = -EINVAL;
 311             goto out;
 312         }
 313     else if (fCount==11 )
 314         {
 315             struct in_device *in_dev = rcu_dereference(dev2->ip_ptr);
 316             __be32 ifaddr = 0;
 317             struct in_ifaddr *ifap;
 318             // dev has a list of IP addresses (because an interface can have multiple)
 319             dnat_enabled.eth2 = 1;
 320             for (ifap = in_dev->ifa_list; ifap != NULL;
 321                     ifap = ifap->ifa_next)
 322                 {
 323                     ifaddr = ifap->ifa_address; // is the IPv4 address
 324                 }
 325             if (ifaddr == 0)
 326                 {
 327                     printk("%s interface ip address list is null!\n",t_name);
 328                     err = -ENODEV;
 329                     goto out;
 330                 }
 331             /* ifa_local: ifa_address is the remote point in ppp */
 332
 333             s_octet1[0] = ((u8 *)&ifaddr)[0];
 334             s_octet1[1] = ((u8 *)&ifaddr)[1];
 335             s_octet1[2] = ((u8 *)&ifaddr)[2];
 336             s_octet1[3] = ((u8 *)&ifaddr)[3];
 337
 338             s_mac1[0] = dev2->dev_addr[0];
 339             s_mac1[1] = dev2->dev_addr[1];
 340             s_mac1[2] = dev2->dev_addr[2];
 341             s_mac1[3] = dev2->dev_addr[3];
 342             s_mac1[4] = dev2->dev_addr[4];
 343             s_mac1[5] = dev2->dev_addr[5];
 344
 345 #ifdef NEVER
 346             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]);
 347             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]);
 348         }
 349     else if (fCount==1 )
 350         {
 351             printk("Level 2 forward enabled for %s\n",t_name);
 352 #endif
 353         }
 354
 355     rtnl_lock();
 356     err = netdev_rx_handler_register(dev1, netdev_frame_hook, dev2);
 357     if (err)
 358         {
 359             printk("can't register rx_handler for device %s\n", net1);
 360             goto out_dev2;
 361         }
 362     dev_set_promiscuity(dev1, 1);
 363
 364     err = netdev_rx_handler_register(dev2, netdev_frame_hook, dev1);
 365     if (err)
 366         {
 367             printk("can't register rx_handler for device %s\n", net2);
 368             netdev_rx_handler_unregister(dev1);
 369             goto out_dev2;
 370         }
 371
 372     dev_set_promiscuity(dev2, 1);
 373
 374     rtnl_unlock();
 375     return 0;
 376
 377 out_dev2:
 378     rtnl_unlock();
 379     dev_put(dev2);
 380     dev2 = NULL;
 381 out_dev1:
 382     dev_put(dev1);
 383     dev1 = NULL;
 384 out:
 385     return err;
 386 }
 387
 388 static void __exit l2fwd_exit_module(void)
 389 {
 390     rtnl_lock();
 391     if (dev2)
 392         {
 393             dev_put(dev2);
 394             netdev_rx_handler_unregister(dev1);
 395         }
 396     if (dev1)
 397         {
 398             dev_put(dev1);
 399             netdev_rx_handler_unregister(dev2);
 400         }
 401     dev_set_promiscuity(dev1, -1);
 402     dev_set_promiscuity(dev2, -1);
 403     rtnl_unlock();
 404 }
 405
 406 module_init(l2fwd_init_module);
 407 module_exit(l2fwd_exit_module);
 408
 409 MODULE_DESCRIPTION("Huawei L2 Forwarding module with DNAT");
 410 MODULE_LICENSE("GPL");
 411 MODULE_VERSION("1.0");