+ feip = hash_get (lcm->fwd_entry_by_mapping_index, dst_map_index);
+ if (!feip)
+ return;
+
+ fe = pool_elt_at_index (lcm->fwd_entry_pool, feip[0]);
+
+ /* delete dp fwd entry */
+ u32 sw_if_index;
+ a->is_add = 0;
+ a->locator_pairs = fe->locator_pairs;
+ a->vni = gid_address_vni (&a->rmt_eid);
+ gid_address_copy (&a->rmt_eid, &fe->deid);
+
+ vnet_lisp_gpe_add_del_fwd_entry (a, &sw_if_index);
+
+ /* delete entry in fwd table */
+ hash_unset (lcm->fwd_entry_by_mapping_index, dst_map_index);
+ vec_free (fe->locator_pairs);
+ pool_put (lcm->fwd_entry_pool, fe);
+}
+
+/**
+ * Finds first remote locator with best (lowest) priority that has a local
+ * peer locator with an underlying route to it.
+ *
+ */
+static u32
+get_locator_pairs (lisp_cp_main_t * lcm, mapping_t * lcl_map,
+ mapping_t * rmt_map, locator_pair_t ** locator_pairs)
+{
+ u32 i, limitp = 0, li, found = 0, esi;
+ locator_set_t *rmt_ls, *lcl_ls;
+ ip_address_t _lcl_addr, *lcl_addr = &_lcl_addr;
+ locator_t *lp, *rmt = 0;
+ uword *checked = 0;
+ locator_pair_t pair;
+
+ rmt_ls =
+ pool_elt_at_index (lcm->locator_set_pool, rmt_map->locator_set_index);
+ lcl_ls =
+ pool_elt_at_index (lcm->locator_set_pool, lcl_map->locator_set_index);
+
+ if (!rmt_ls || vec_len (rmt_ls->locator_indices) == 0)
+ return 0;
+
+ while (1)
+ {
+ rmt = 0;
+
+ /* find unvisited remote locator with best priority */
+ for (i = 0; i < vec_len (rmt_ls->locator_indices); i++)
+ {
+ if (0 != hash_get (checked, i))
+ continue;
+
+ li = vec_elt (rmt_ls->locator_indices, i);
+ lp = pool_elt_at_index (lcm->locator_pool, li);
+
+ /* we don't support non-IP locators for now */
+ if (gid_address_type (&lp->address) != GID_ADDR_IP_PREFIX)
+ continue;
+
+ if ((found && lp->priority == limitp)
+ || (!found && lp->priority >= limitp))
+ {
+ rmt = lp;
+
+ /* don't search for locators with lower priority and don't
+ * check this locator again*/
+ limitp = lp->priority;
+ hash_set (checked, i, 1);
+ break;
+ }
+ }
+ /* check if a local locator with a route to remote locator exists */
+ if (rmt != 0)
+ {
+ /* find egress sw_if_index for rmt locator */
+ esi =
+ ip_fib_get_egress_iface_for_dst (lcm,
+ &gid_address_ip (&rmt->address));
+ if ((u32) ~ 0 == esi)
+ continue;
+
+ for (i = 0; i < vec_len (lcl_ls->locator_indices); i++)
+ {
+ li = vec_elt (lcl_ls->locator_indices, i);
+ locator_t *sl = pool_elt_at_index (lcm->locator_pool, li);
+
+ /* found local locator with the needed sw_if_index */
+ if (sl->sw_if_index == esi)
+ {
+ /* and it has an address */
+ if (0 == ip_interface_get_first_ip_address (lcm,
+ sl->sw_if_index,
+ gid_address_ip_version
+ (&rmt->address),
+ lcl_addr))
+ continue;
+
+ memset (&pair, 0, sizeof (pair));
+ ip_address_copy (&pair.rmt_loc,
+ &gid_address_ip (&rmt->address));
+ ip_address_copy (&pair.lcl_loc, lcl_addr);
+ pair.weight = rmt->weight;
+ vec_add1 (locator_pairs[0], pair);
+ found = 1;
+ }
+ }
+ }
+ else
+ break;
+ }
+
+ hash_free (checked);
+ return found;
+}
+
+static void
+dp_add_fwd_entry (lisp_cp_main_t * lcm, u32 src_map_index, u32 dst_map_index)
+{
+ vnet_lisp_gpe_add_del_fwd_entry_args_t _a, *a = &_a;
+ mapping_t *src_map, *dst_map;
+ u32 sw_if_index;
+ uword *feip = 0, *dpid;
+ fwd_entry_t *fe;
+ u8 type;
+
+ memset (a, 0, sizeof (*a));
+
+ /* remove entry if it already exists */
+ feip = hash_get (lcm->fwd_entry_by_mapping_index, dst_map_index);
+ if (feip)
+ dp_del_fwd_entry (lcm, src_map_index, dst_map_index);
+
+ src_map = pool_elt_at_index (lcm->mapping_pool, src_map_index);
+ dst_map = pool_elt_at_index (lcm->mapping_pool, dst_map_index);
+
+ /* insert data plane forwarding entry */
+ a->is_add = 1;
+
+ gid_address_copy (&a->rmt_eid, &dst_map->eid);
+ a->vni = gid_address_vni (&a->rmt_eid);
+
+ /* get vrf or bd_index associated to vni */
+ type = gid_address_type (&dst_map->eid);
+ if (GID_ADDR_IP_PREFIX == type)
+ {
+ dpid = hash_get (lcm->table_id_by_vni, a->vni);
+ if (!dpid)
+ {
+ clib_warning ("vni %d not associated to a vrf!", a->vni);
+ return;
+ }
+ a->table_id = dpid[0];
+ }
+ else if (GID_ADDR_MAC == type)
+ {
+ dpid = hash_get (lcm->bd_id_by_vni, a->vni);
+ if (!dpid)
+ {
+ clib_warning ("vni %d not associated to a bridge domain !", a->vni);
+ return;
+ }
+ a->bd_id = dpid[0];
+ }
+
+ /* find best locator pair that 1) verifies LISP policy 2) are connected */
+ if (0 == get_locator_pairs (lcm, src_map, dst_map, &a->locator_pairs))
+ {
+ /* negative entry */
+ a->is_negative = 1;
+ a->action = dst_map->action;
+ }
+
+ /* TODO remove */
+ u8 ipver = ip_prefix_version (&gid_address_ippref (&a->rmt_eid));
+ a->decap_next_index = (ipver == IP4) ?
+ LISP_GPE_INPUT_NEXT_IP4_INPUT : LISP_GPE_INPUT_NEXT_IP6_INPUT;
+
+ vnet_lisp_gpe_add_del_fwd_entry (a, &sw_if_index);
+
+ /* add tunnel to fwd entry table XXX check return value from DP insertion */
+ pool_get (lcm->fwd_entry_pool, fe);
+ fe->locator_pairs = a->locator_pairs;
+ gid_address_copy (&fe->deid, &a->rmt_eid);
+ hash_set (lcm->fwd_entry_by_mapping_index, dst_map_index,
+ fe - lcm->fwd_entry_pool);
+}
+
+/**
+ * Add/remove mapping to/from map-cache. Overwriting not allowed.
+ */