LISP: Enhance IPx offset computing

[vpp.git] / src / vnet / lisp-cp / control.c

diff --git a/src/vnet/lisp-cp/control.c b/src/vnet/lisp-cp/control.c
index de048a4..b67d0e9 100644 (file)
--- a/src/vnet/lisp-cp/control.c
+++ b/src/vnet/lisp-cp/control.c
@@ -512,12 +512,46 @@ dp_add_fwd_entry (lisp_cp_main_t * lcm, u32 src_map_index, u32 dst_map_index)
   pool_get (lcm->fwd_entry_pool, fe);
   fe->locator_pairs = a->locator_pairs;
   gid_address_copy (&fe->reid, &a->rmt_eid);
-  gid_address_copy (&fe->leid, &src_map->eid);
+
+  if (is_src_dst)
+    gid_address_copy (&fe->leid, &a->lcl_eid);
+  else
+    gid_address_copy (&fe->leid, &src_map->eid);
+
   fe->is_src_dst = is_src_dst;
   hash_set (lcm->fwd_entry_by_mapping_index, dst_map_index,
            fe - lcm->fwd_entry_pool);
 }
 
+typedef struct
+{
+  u32 si;
+  u32 di;
+} fwd_entry_mt_arg_t;
+
+static void *
+dp_add_fwd_entry_thread_fn (void *arg)
+{
+  fwd_entry_mt_arg_t *a = arg;
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  dp_add_fwd_entry (lcm, a->si, a->di);
+  return 0;
+}
+
+static int
+dp_add_fwd_entry_from_mt (u32 si, u32 di)
+{
+  fwd_entry_mt_arg_t a;
+
+  memset (&a, 0, sizeof (a));
+  a.si = si;
+  a.di = di;
+
+  vl_api_rpc_call_main_thread (dp_add_fwd_entry_thread_fn,
+                              (u8 *) & a, sizeof (a));
+  return 0;
+}
+
 /**
  * Returns vector of adjacencies.
  *
@@ -3950,7 +3984,7 @@ lisp_cp_lookup_inline (vlib_main_t * vm,
                                              &src);
                  if (~0 != si)
                    {
-                     dp_add_fwd_entry (lcm, si, di);
+                     dp_add_fwd_entry_from_mt (si, di);
                    }
                }
            }
@@ -4457,13 +4491,26 @@ send_map_reply (lisp_cp_main_t * lcm, u32 mi, ip_address_t * dst,
   return 0;
 }
 
+static void
+find_ip_header (vlib_buffer_t * b, u8 ** ip_hdr)
+{
+  const i32 start = vnet_buffer (b)->ip.start_of_ip_header;
+  if (start < 0 && start < -sizeof (b->pre_data))
+    {
+      *ip_hdr = 0;
+      return;
+    }
+
+  *ip_hdr = b->data + start;
+  if ((u8 *) * ip_hdr > (u8 *) vlib_buffer_get_current (b))
+    *ip_hdr = 0;
+}
+
 void
 process_map_request (vlib_main_t * vm, lisp_cp_main_t * lcm,
                     vlib_buffer_t * b)
 {
-  u8 *ip_hdr = 0, *udp_hdr;
-  ip4_header_t *ip4;
-  ip6_header_t *ip6;
+  u8 *ip_hdr = 0;
   ip_address_t *dst_loc = 0, probed_loc, src_loc;
   mapping_t m;
   map_request_hdr_t *mreq_hdr;
@@ -4473,33 +4520,6 @@ process_map_request (vlib_main_t * vm, lisp_cp_main_t * lcm,
   gid_address_t *itr_rlocs = 0;
 
   mreq_hdr = vlib_buffer_get_current (b);
-
-  // TODO ugly workaround to find out whether LISP is carried by ip4 or 6
-  // and needs to be fixed
-  udp_hdr = (u8 *) vlib_buffer_get_current (b) - sizeof (udp_header_t);
-  ip4 = (ip4_header_t *) (udp_hdr - sizeof (ip4_header_t));
-  ip6 = (ip6_header_t *) (udp_hdr - sizeof (ip6_header_t));
-
-  if ((ip4->ip_version_and_header_length & 0xF0) == 0x40)
-    ip_hdr = (u8 *) ip4;
-  else
-    {
-      u32 flags = clib_net_to_host_u32
-       (ip6->ip_version_traffic_class_and_flow_label);
-      if ((flags & 0xF0000000) == 0x60000000)
-       ip_hdr = (u8 *) ip6;
-      else
-       {
-         clib_warning ("internal error: cannot determine whether packet "
-                       "is ip4 or 6!");
-         return;
-       }
-    }
-
-  vlib_buffer_pull (b, sizeof (*mreq_hdr));
-
-  nonce = MREQ_NONCE (mreq_hdr);
-
   if (!MREQ_SMR (mreq_hdr) && !MREQ_RLOC_PROBE (mreq_hdr))
     {
       clib_warning
@@ -4507,6 +4527,9 @@ process_map_request (vlib_main_t * vm, lisp_cp_main_t * lcm,
       return;
     }
 
+  vlib_buffer_pull (b, sizeof (*mreq_hdr));
+  nonce = MREQ_NONCE (mreq_hdr);
+
   /* parse src eid */
   len = lisp_msg_parse_addr (b, &src);
   if (len == ~0)
@@ -4535,6 +4558,13 @@ process_map_request (vlib_main_t * vm, lisp_cp_main_t * lcm,
        }
       else if (MREQ_RLOC_PROBE (mreq_hdr))
        {
+         find_ip_header (b, &ip_hdr);
+         if (!ip_hdr)
+           {
+             clib_warning ("Cannot find the IP header!");
+             return;
+           }
+
          memset (&m, 0, sizeof (m));
          u32 mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &dst);
 
@@ -4602,7 +4632,7 @@ parse_map_reply (vlib_buffer_t * b)
 static void
 queue_map_reply_for_processing (map_records_arg_t * a)
 {
-  vl_api_rpc_call_main_thread (process_map_reply, (u8 *) a, sizeof (a));
+  vl_api_rpc_call_main_thread (process_map_reply, (u8 *) a, sizeof (*a));
 }
 
 static void