Add support for MPLS labels in router plugin

1. Fixed more formatting issues. Added ASSERT.
2. Added MPLS features to support iproute2 command like
ip -f mpls route add <label> dev vpp0
ip route add <IP> encap mpls <LABELS> via <IP> dev vpp0
3. Modifications to compile with the latest VPP.

Change-Id: I5ed55ec3c6484beb1377496e9c763bb55da7ee26
Signed-off-by: Chad Wang <imflinter@gmail.com>

2 files changed, 145 insertions, 61 deletions

diff --git a/router/README.md b/router/README.md
index 3401c76..b3ec5ae 100644
--- a/router/README.md
+++ b/router/README.md
@@ -66,6 +66,30 @@ The objective of this project is to continue to build out better integration
 with host operating system and for providing a basis to enable completely
 or partially unmodified applications to take advantage of a fast datapath.
 
+### MPLS related enhancement to router plugin
+
+Some changes made to router plugin enable it to support mpls label encapsulation.
+
+To test this feature, first install router plugin using instructions
+in section "Build/Install", and then run following commands
+
+$vppctl enable tap-inject
+$ifconfig vpp0 <IP> up
+$ip route add <IP> encap mpls <LABELS> via <IP> dev vpp0
+$vppctl show ip fib
+
+You should see labels in ip fib table.
+
+you can also run following commands
+
+$vppctl enable tap-inject
+$vppctl mpls table add 0
+$vppctl set int mpls <INTERFACE> enable
+$ip -f mpls route add 333 dev vpp0
+$vppctl show mpls fib
+
+You should see dst label and interface in mpls table.
+
 ### Main contributors
 
-Jeff Shaw - LF-ID:jbshaw
+Jeff Shaw - LF-ID:jbshaw, Chad Chengwei Wang - LF-ID:flinter (MPLS)
diff --git a/router/router/tap_inject_netlink.c b/router/router/tap_inject_netlink.c
index 19d5d04..3dca013 100644
--- a/router/router/tap_inject_netlink.c
+++ b/router/router/tap_inject_netlink.c
@@ -15,7 +15,6 @@
  */
 
 #include "tap_inject.h"
-
 #include <librtnl/netns.h>
 #include <vlibmemory/api.h>
 #include <vnet/ethernet/arp_packet.h>
@@ -30,6 +29,10 @@
 #define FIB_VERSION 2
 #endif
 
+#include <arpa/inet.h>
+#include <linux/mpls.h>
+#include <vnet/mpls/packet.h>
+
 static void
 add_del_addr (ns_addr_t * a, int is_del)
 {
@@ -37,7 +40,7 @@ add_del_addr (ns_addr_t * a, int is_del)
   u32 sw_if_index;
 
   sw_if_index = tap_inject_lookup_sw_if_index_from_tap_if_index (
-                        a->ifaddr.ifa_index);
+                                                                 a->ifaddr.ifa_index);
 
   if (sw_if_index == ~0)
     return;
@@ -45,12 +48,12 @@ add_del_addr (ns_addr_t * a, int is_del)
   if (a->ifaddr.ifa_family == AF_INET)
     {
       ip4_add_del_interface_address (vm, sw_if_index,
-          (ip4_address_t *) a->local, a->ifaddr.ifa_prefixlen, is_del);
+                                     (ip4_address_t *) a->local, a->ifaddr.ifa_prefixlen, is_del);
     }
   else if (a->ifaddr.ifa_family == AF_INET6)
     {
       ip6_add_del_interface_address (vm, sw_if_index,
-          (ip6_address_t *) a->addr, a->ifaddr.ifa_prefixlen, is_del);
+                                     (ip6_address_t *) a->addr, a->ifaddr.ifa_prefixlen, is_del);
     }
 }
 
@@ -75,7 +78,7 @@ add_del_link (ns_link_t * l, int is_del)
   u32 sw_if_index;
 
   sw_if_index = tap_inject_lookup_sw_if_index_from_tap_if_index (
-                        l->ifi.ifi_index);
+                                                                 l->ifi.ifi_index);
 
   if (sw_if_index == ~0)
     return;
@@ -104,7 +107,7 @@ add_del_neigh (ns_neigh_t * n, int is_del)
   u32 sw_if_index;
 
   sw_if_index = tap_inject_lookup_sw_if_index_from_tap_if_index (
-                        n->nd.ndm_ifindex);
+                                                                 n->nd.ndm_ifindex);
 
   if (sw_if_index == ~0)
     return;
@@ -122,40 +125,40 @@ add_del_neigh (ns_neigh_t * n, int is_del)
       if (n->nd.ndm_state & NUD_REACHABLE)
         {
 #if FIB_VERSION == 1
-        vnet_arp_set_ip4_over_ethernet (vnet_main, sw_if_index, ~0, &a, 0);
+          vnet_arp_set_ip4_over_ethernet (vnet_main, sw_if_index, ~0, &a, 0);
 #else
-        vnet_arp_set_ip4_over_ethernet (vnet_main, sw_if_index,
-                                        &a, 0 /* static */ ,
-                                        0 /* no fib entry */);
+          vnet_arp_set_ip4_over_ethernet (vnet_main, sw_if_index,
+                                          &a, 0 /* static */ ,
+                                          0 /* no fib entry */);
 
 #endif /* FIB_VERSION == 1 */
         }
       else if (n->nd.ndm_state & NUD_FAILED)
         {
 #if FIB_VERSION == 1
-        vnet_arp_unset_ip4_over_ethernet (vnet_main, sw_if_index, ~0, &a);
+          vnet_arp_unset_ip4_over_ethernet (vnet_main, sw_if_index, ~0, &a);
 #else
-        vnet_arp_unset_ip4_over_ethernet (vnet_main, sw_if_index, &a);
+          vnet_arp_unset_ip4_over_ethernet (vnet_main, sw_if_index, &a);
 #endif /* FIB_VERSION == 1 */
         }
     }
   else if (n->nd.ndm_family == AF_INET6)
     {
       if (n->nd.ndm_state & NUD_REACHABLE)
-	{
+        {
 #if FIB_VERSION == 1
-        vnet_set_ip6_ethernet_neighbor (vm, sw_if_index,
-            (ip6_address_t *) n->dst, n->lladdr, ETHER_ADDR_LEN, 0);
+          vnet_set_ip6_ethernet_neighbor (vm, sw_if_index,
+                                          (ip6_address_t *) n->dst, n->lladdr, ETHER_ADDR_LEN, 0);
 #else
-        vnet_set_ip6_ethernet_neighbor (vm, sw_if_index,
-            (ip6_address_t *) n->dst, n->lladdr, ETHER_ADDR_LEN,
-            0 /* static */,
-            0 /* no fib entry */);
+          vnet_set_ip6_ethernet_neighbor (vm, sw_if_index,
+                                          (ip6_address_t *) n->dst, n->lladdr, ETHER_ADDR_LEN,
+                                          0 /* static */,
+                                          0 /* no fib entry */);
 #endif /* FIB_VERSION == 1 */
-	}
+        }
       else
         vnet_unset_ip6_ethernet_neighbor (vm, sw_if_index,
-            (ip6_address_t *) n->dst, n->lladdr, ETHER_ADDR_LEN);
+                                          (ip6_address_t *) n->dst, n->lladdr, ETHER_ADDR_LEN);
     }
 }
 
@@ -163,67 +166,124 @@ add_del_neigh (ns_neigh_t * n, int is_del)
 #define TAP_INJECT_HOST_ROUTE_TABLE_MAIN 254
 
 static void
+get_mpls_label_stack(struct mpls_label *addr, u32* l)
+{
+  u32 entry = ntohl(addr[0].entry);
+  u32 label = (entry & MPLS_LS_LABEL_MASK) >> MPLS_LS_LABEL_SHIFT;
+
+  for(int i = 1; label != 0; i++) {
+    *l++ = label;
+    if(entry & MPLS_LS_S_MASK)
+      return;
+    entry = ntohl(addr[i].entry);
+    label = (entry & MPLS_LS_LABEL_MASK) >> MPLS_LS_LABEL_SHIFT;
+  }
+}
+
+static void
 add_del_route (ns_route_t * r, int is_del)
 {
   u32 sw_if_index;
 
   sw_if_index = tap_inject_lookup_sw_if_index_from_tap_if_index (r->oif);
 
-  if (sw_if_index == ~0 || r->table != TAP_INJECT_HOST_ROUTE_TABLE_MAIN)
+  if (sw_if_index == ~0)
     return;
 
   if (r->rtm.rtm_family == AF_INET)
     {
+      u32 stack[MPLS_STACK_DEPTH] = {0};
+
 #if FIB_VERSION == 1
       ip4_add_del_route_next_hop (&ip4_main,
-          is_del ? IP4_ROUTE_FLAG_DEL : IP4_ROUTE_FLAG_ADD,
-          (ip4_address_t *) r->dst, r->rtm.rtm_dst_len,
-          (ip4_address_t *) r->gateway, sw_if_index, 0, ~0, 0);
+                                  is_del ? IP4_ROUTE_FLAG_DEL : IP4_ROUTE_FLAG_ADD,
+                                  (ip4_address_t *) r->dst, r->rtm.rtm_dst_len,
+                                  (ip4_address_t *) r->gateway, sw_if_index, 0, ~0, 0);
 #else
-	    fib_prefix_t prefix;
-	    ip46_address_t nh;
-
-	    memset (&prefix, 0, sizeof (prefix));
-	    prefix.fp_len = r->rtm.rtm_dst_len;
-	    prefix.fp_proto = FIB_PROTOCOL_IP4;
-	    clib_memcpy (&prefix.fp_addr.ip4, r->dst, sizeof (prefix.fp_addr.ip4));
-
-	    memset (&nh, 0, sizeof (nh));
-	    clib_memcpy (&nh.ip4, r->gateway, sizeof (nh.ip4));
-
-	    fib_table_entry_path_add (0, &prefix, FIB_SOURCE_API,
-	                              FIB_ENTRY_FLAG_NONE, prefix.fp_proto,
-	                              &nh, sw_if_index, 0,
-	                              0 /* weight */, NULL,
-	                              FIB_ROUTE_PATH_FLAG_NONE);
+      fib_prefix_t prefix;
+      ip46_address_t nh;
+
+      memset (&prefix, 0, sizeof (prefix));
+      prefix.fp_len = r->rtm.rtm_dst_len;
+      prefix.fp_proto = FIB_PROTOCOL_IP4;
+      clib_memcpy (&prefix.fp_addr.ip4, r->dst, sizeof (prefix.fp_addr.ip4));
+      get_mpls_label_stack(r->encap, stack);
+      memset (&nh, 0, sizeof (nh));
+      clib_memcpy (&nh.ip4, r->gateway, sizeof (nh.ip4));
+      if(*stack == 0)
+        fib_table_entry_path_add (0, &prefix, FIB_SOURCE_API,
+                                  FIB_ENTRY_FLAG_NONE, prefix.fp_proto,
+                                  &nh, sw_if_index, 0,
+                                  0 /* weight */, NULL,
+                                  FIB_ROUTE_PATH_FLAG_NONE);
+      else {
+        fib_route_path_t *rpaths = NULL, rpath;
+        memset(&rpath, 0, sizeof(rpath));
+        rpath.frp_weight = 1;
+        rpath.frp_proto = DPO_PROTO_IP4;
+        clib_memcpy(&rpath.frp_addr.ip4, r->gateway, sizeof(rpath.frp_addr.ip4));
+        rpath.frp_sw_if_index = sw_if_index;
+        for(int i = 0; i < MPLS_STACK_DEPTH && stack[i] != 0; i++) {
+          fib_mpls_label_t fib_label = {stack[i],0,0,0};
+          vec_add1(rpath.frp_label_stack, fib_label);
+        }
+        vec_add1(rpaths, rpath);
+        fib_table_entry_path_add2(0,
+                                  &prefix,
+                                  FIB_SOURCE_API,
+                                  FIB_ENTRY_FLAG_NONE,
+                                  rpaths);
+      }
 #endif /* FIB_VERSION == 1 */
     }
   else if (r->rtm.rtm_family == AF_INET6)
     {
 #if FIB_VERSION == 1
       ip6_add_del_route_next_hop (&ip6_main,
-          is_del ? IP6_ROUTE_FLAG_DEL : IP6_ROUTE_FLAG_ADD,
-          (ip6_address_t *) r->dst, r->rtm.rtm_dst_len,
-          (ip6_address_t *) r->gateway, sw_if_index, 0, ~0, 0);
+                                  is_del ? IP6_ROUTE_FLAG_DEL : IP6_ROUTE_FLAG_ADD,
+                                  (ip6_address_t *) r->dst, r->rtm.rtm_dst_len,
+                                  (ip6_address_t *) r->gateway, sw_if_index, 0, ~0, 0);
 #else
-	    fib_prefix_t prefix;
-	    ip46_address_t nh;
-
-	    memset (&prefix, 0, sizeof (prefix));
-	    prefix.fp_len = r->rtm.rtm_dst_len;
-	    prefix.fp_proto = FIB_PROTOCOL_IP6;
-	    clib_memcpy (&prefix.fp_addr.ip6, r->dst, sizeof (prefix.fp_addr.ip6));
-
-	    memset (&nh, 0, sizeof (nh));
-	    clib_memcpy (&nh.ip6, r->gateway, sizeof (nh.ip6));
-
-	    fib_table_entry_path_add (0, &prefix, FIB_SOURCE_API,
-	                              FIB_ENTRY_FLAG_NONE, prefix.fp_proto,
-	                              &nh, sw_if_index, 0,
-	                              0 /* weight */, NULL,
-	                              FIB_ROUTE_PATH_FLAG_NONE);
+      fib_prefix_t prefix;
+      ip46_address_t nh;
+      memset (&prefix, 0, sizeof (prefix));
+      prefix.fp_len = r->rtm.rtm_dst_len;
+      prefix.fp_proto = FIB_PROTOCOL_IP6;
+      clib_memcpy (&prefix.fp_addr.ip6, r->dst, sizeof (prefix.fp_addr.ip6));
+      memset (&nh, 0, sizeof (nh));
+      clib_memcpy (&nh.ip6, r->gateway, sizeof (nh.ip6));
+      fib_table_entry_path_add (0, &prefix, FIB_SOURCE_API,
+                                FIB_ENTRY_FLAG_NONE, prefix.fp_proto,
+                                &nh, sw_if_index, 0,
+                                0 /* weight */, NULL,
+                                FIB_ROUTE_PATH_FLAG_NONE);
 #endif /* FIB_VERSION == 1 */
     }
+  else if (r->rtm.rtm_family == AF_MPLS)
+    {
+      u32 dst_label;
+      get_mpls_label_stack((struct mpls_label*) r->dst, &dst_label);
+      struct rtvia *via = (struct rtvia*) r->via;
+      fib_prefix_t prefix;
+      fib_route_path_t *rpaths = NULL, rpath;
+      memset (&prefix, 0, sizeof (prefix));
+      prefix.fp_len = 21;
+      prefix.fp_label = dst_label;
+      prefix.fp_proto = FIB_PROTOCOL_MPLS;
+      prefix.fp_payload_proto = DPO_PROTO_IP4;
+      memset(&rpath, 0, sizeof(rpath));
+      clib_memcpy (&rpath.frp_addr.ip4, via->rtvia_addr, sizeof (rpath.frp_addr.ip4));
+      rpath.frp_weight = 1;
+      rpath.frp_proto = DPO_PROTO_IP4;
+      rpath.frp_fib_index = 0;
+      rpath.frp_sw_if_index = sw_if_index;
+      vec_add1(rpaths, rpath);
+      fib_table_entry_path_add2(0,
+                                &prefix,
+                                FIB_SOURCE_API,
+                                FIB_ENTRY_FLAG_NONE,
+                                rpaths);
+    }
 }