vnet/vnet/lisp-cp/control.c

   1 /*
   2  * Copyright (c) 2016 Cisco and/or its affiliates.
   3  * Licensed under the Apache License, Version 2.0 (the "License");
   4  * you may not use this file except in compliance with the License.
   5  * You may obtain a copy of the License at:
   6  *
   7  *     http://www.apache.org/licenses/LICENSE-2.0
   8  *
   9  * Unless required by applicable law or agreed to in writing, software
  10  * distributed under the License is distributed on an "AS IS" BASIS,
  11  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  12  * See the License for the specific language governing permissions and
  13  * limitations under the License.
  14  */
  15
  16 #include <vnet/lisp-cp/control.h>
  17 #include <vnet/lisp-cp/packets.h>
  18 #include <vnet/lisp-cp/lisp_msg_serdes.h>
  19 #include <vnet/lisp-gpe/lisp_gpe.h>
  20
  21 static void
  22 add_fwd_entry (lisp_cp_main_t* lcm, u32 src_map_index, u32 dst_map_index);
  23
  24 static void
  25 del_fwd_entry (lisp_cp_main_t * lcm, u32 src_map_index, u32 dst_map_index);
  26
  27 static u8
  28 compare_locators (lisp_cp_main_t *lcm, u32 * old_ls_indexes,
  29                   locator_t * new_locators);
  30
  31 /* Stores mapping in map-cache. It does NOT program data plane forwarding for
  32  * remote/learned mappings. */
  33 int
  34 vnet_lisp_add_del_mapping (vnet_lisp_add_del_mapping_args_t * a,
  35                            u32 * map_index_result)
  36 {
  37   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
  38   u32 mi, * map_indexp, map_index, i;
  39   mapping_t * m, * old_map;
  40   u32 ** eid_indexes;
  41
  42   mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &a->deid);
  43   old_map = mi != ~0 ? pool_elt_at_index (lcm->mapping_pool, mi) : 0;
  44   if (a->is_add)
  45     {
  46       /* TODO check if overwriting and take appropriate actions */
  47       if (mi != GID_LOOKUP_MISS && !gid_address_cmp (&old_map->eid,
  48                                                      &a->deid))
  49         {
  50           clib_warning("eid %U found in the eid-table", format_ip_address,
  51                        &a->deid);
  52           return VNET_API_ERROR_VALUE_EXIST;
  53         }
  54
  55       pool_get(lcm->mapping_pool, m);
  56       m->eid = a->deid;
  57       m->locator_set_index = a->locator_set_index;
  58       m->ttl = a->ttl;
  59       m->action = a->action;
  60       m->local = a->local;
  61
  62       map_index = m - lcm->mapping_pool;
  63       gid_dictionary_add_del (&lcm->mapping_index_by_gid, &a->deid, map_index,
  64                               1);
  65
  66       if (pool_is_free_index(lcm->locator_set_pool, a->locator_set_index))
  67         {
  68           clib_warning("Locator set with index %d doesn't exist",
  69                        a->locator_set_index);
  70           return VNET_API_ERROR_INVALID_VALUE;
  71         }
  72
  73       /* add eid to list of eids supported by locator-set */
  74       vec_validate (lcm->locator_set_to_eids, a->locator_set_index);
  75       eid_indexes = vec_elt_at_index(lcm->locator_set_to_eids,
  76                                      a->locator_set_index);
  77       vec_add1(eid_indexes[0], map_index);
  78
  79       if (a->local)
  80         {
  81           /* mark as local */
  82           vec_add1(lcm->local_mappings_indexes, map_index);
  83         }
  84       map_index_result[0] = map_index;
  85     }
  86   else
  87     {
  88       if (mi == GID_LOOKUP_MISS)
  89         {
  90           clib_warning("eid %U not found in the eid-table", format_ip_address,
  91                        &a->deid);
  92           return VNET_API_ERROR_INVALID_VALUE;
  93         }
  94
  95       /* clear locator-set to eids binding */
  96       eid_indexes = vec_elt_at_index(lcm->locator_set_to_eids,
  97                                      a->locator_set_index);
  98       for (i = 0; i < vec_len(eid_indexes[0]); i++)
  99         {
 100           map_indexp = vec_elt_at_index(eid_indexes[0], i);
 101           if (map_indexp[0] == mi)
 102               break;
 103         }
 104       vec_del1(eid_indexes[0], i);
 105
 106       /* remove local mark if needed */
 107       m = pool_elt_at_index(lcm->mapping_pool, mi);
 108       if (m->local)
 109         {
 110           u32 k, * lm_indexp;
 111           for (k = 0; k < vec_len(lcm->local_mappings_indexes); k++)
 112             {
 113               lm_indexp = vec_elt_at_index(lcm->local_mappings_indexes, k);
 114               if (lm_indexp[0] == mi)
 115                 break;
 116             }
 117           vec_del1(lcm->local_mappings_indexes, k);
 118         }
 119       else
 120         {
 121           /* remove tunnel ??? */
 122         }
 123
 124       /* remove mapping from dictionary */
 125       gid_dictionary_add_del (&lcm->mapping_index_by_gid, &a->deid, 0, 0);
 126       pool_put_index (lcm->mapping_pool, mi);
 127     }
 128
 129   return 0;
 130 }
 131
 132 /* Stores mapping in map-cache and programs data plane for local mappings. */
 133 int
 134 vnet_lisp_add_del_local_mapping (vnet_lisp_add_del_mapping_args_t * a,
 135                                  u32 * map_index_result)
 136 {
 137   uword * table_id, * refc;
 138   u32 rv, vni;
 139   vnet_lisp_gpe_add_del_iface_args_t _ai, *ai = &_ai;
 140   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
 141
 142   vni = gid_address_vni(&a->deid);
 143
 144   /* store/remove mapping from map-cache */
 145   rv = vnet_lisp_add_del_mapping (a, map_index_result);
 146   if (rv)
 147     return rv;
 148
 149   table_id = hash_get(lcm->table_id_by_vni, vni);
 150
 151   if (!table_id)
 152     {
 153       clib_warning ("vni %d not associated to a vrf!", vni);
 154       return VNET_API_ERROR_INVALID_VALUE;
 155     }
 156
 157   refc = hash_get(lcm->dp_if_refcount_by_vni, vni);
 158
 159   /* enable/disable data-plane interface */
 160   if (a->is_add)
 161     {
 162       /* create interface or update refcount */
 163       if (!refc)
 164         {
 165           ai->is_add = 1;
 166           ai->vni = vni;
 167           ai->table_id = table_id[0];
 168           vnet_lisp_gpe_add_del_iface (ai, 0);
 169
 170           /* counts the number of eids in a vni that use the interface */
 171           hash_set(lcm->dp_if_refcount_by_vni, vni, 1);
 172         }
 173       else
 174         {
 175           refc[0]++;
 176         }
 177     }
 178   else
 179     {
 180       /* since this is a remove for an existing eid, the iface should exist */
 181       ASSERT(refc != 0);
 182       refc[0]--;
 183
 184       /* remove iface if needed */
 185       if (refc[0] == 0)
 186         {
 187           ai->is_add = 0;
 188           ai->vni = vni;
 189           ai->table_id = table_id[0];
 190           vnet_lisp_gpe_add_del_iface (ai, 0);
 191           hash_unset (lcm->dp_if_refcount_by_vni, vni);
 192         }
 193     }
 194
 195   return rv;
 196 }
 197
 198 static clib_error_t *
 199 lisp_add_del_local_eid_command_fn (vlib_main_t * vm, unformat_input_t * input,
 200                                    vlib_cli_command_t * cmd)
 201 {
 202   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 203   unformat_input_t _line_input, * line_input = &_line_input;
 204   u8 is_add = 1;
 205   gid_address_t eid;
 206   ip_prefix_t * prefp = &gid_address_ippref(&eid);
 207   gid_address_t * eids = 0;
 208   clib_error_t * error = 0;
 209   u8 * locator_set_name = 0;
 210   u32 locator_set_index = 0, map_index = 0;
 211   uword * p;
 212   vnet_lisp_add_del_mapping_args_t _a, * a = &_a;
 213
 214   gid_address_type (&eid) = GID_ADDR_IP_PREFIX;
 215
 216   /* Get a line of input. */
 217   if (! unformat_user (input, unformat_line_input, line_input))
 218     return 0;
 219
 220   while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
 221     {
 222       if (unformat (line_input, "add"))
 223         is_add = 1;
 224       else if (unformat (line_input, "del"))
 225         is_add = 0;
 226       else if (unformat (line_input, "eid %U", unformat_ip_prefix, prefp))
 227         {
 228           vec_add1(eids, eid);
 229         }
 230       else if (unformat (line_input, "locator-set %_%v%_", &locator_set_name))
 231         {
 232           p = hash_get_mem(lcm->locator_set_index_by_name, locator_set_name);
 233           if (!p)
 234             {
 235               error = clib_error_return(0, "locator-set %s doesn't exist",
 236                                         locator_set_name);
 237               goto done;
 238             }
 239           locator_set_index = p[0];
 240         }
 241       else
 242         {
 243           error = unformat_parse_error(line_input);
 244           goto done;
 245         }
 246     }
 247
 248   /* XXX treat batch configuration */
 249   a->deid = eid;
 250   a->is_add = is_add;
 251   a->locator_set_index = locator_set_index;
 252   a->local = 1;
 253
 254   vnet_lisp_add_del_local_mapping (a, &map_index);
 255  done:
 256   vec_free(eids);
 257   if (locator_set_name)
 258     vec_free (locator_set_name);
 259   return error;
 260 }
 261
 262 VLIB_CLI_COMMAND (lisp_add_del_local_eid_command) = {
 263     .path = "lisp eid-table",
 264     .short_help = "lisp eid-table add/del eid <eid> locator-set <locator-set>",
 265     .function = lisp_add_del_local_eid_command_fn,
 266 };
 267
 268 static int
 269 lisp_add_del_negative_static_mapping (gid_address_t * deid,
 270     vnet_lisp_add_del_locator_set_args_t * ls, u8 action, u8 is_add)
 271 {
 272   uword * p;
 273   mapping_t * map;
 274   u32 mi = ~0;
 275   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
 276   uword * refc;
 277   vnet_lisp_add_del_mapping_args_t _dm_args, * dm_args = &_dm_args;
 278   int rv = 0;
 279   u32 ls_index = 0, dst_map_index;
 280   vnet_lisp_gpe_add_del_iface_args_t _ai, *ai = &_ai;
 281
 282   memset (dm_args, 0, sizeof (dm_args[0]));
 283   u32 vni = gid_address_vni (deid);
 284   refc = hash_get (lcm->dp_if_refcount_by_vni, vni);
 285
 286   p = hash_get (lcm->table_id_by_vni, vni);
 287   if (!p)
 288     {
 289       clib_warning ("vni %d not associated to a vrf!", vni);
 290       return VNET_API_ERROR_INVALID_VALUE;
 291     }
 292
 293   if (is_add)
 294     {
 295       vnet_lisp_add_del_locator_set (ls, &ls_index);
 296       /* add mapping */
 297       gid_address_copy (&dm_args->deid, deid);
 298       dm_args->is_add = 1;
 299       dm_args->action = action;
 300       dm_args->locator_set_index = ls_index;
 301
 302       /* create interface or update refcount */
 303       if (!refc)
 304         {
 305           vnet_lisp_gpe_add_del_iface_args_t _ai, *ai = &_ai;
 306           ai->is_add = 1;
 307           ai->vni = vni;
 308           ai->table_id = p[0];
 309           vnet_lisp_gpe_add_del_iface (ai, 0);
 310
 311           /* counts the number of eids in a vni that use the interface */
 312           hash_set (lcm->dp_if_refcount_by_vni, vni, 1);
 313         }
 314       else
 315         refc[0]++;
 316
 317       rv = vnet_lisp_add_del_local_mapping (dm_args, &dst_map_index);
 318       if (!rv)
 319         add_fwd_entry (lcm, lcm->pitr_map_index, dst_map_index);
 320     }
 321   else
 322     {
 323       mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, deid);
 324       if ((u32)~0 == mi)
 325         {
 326           clib_warning ("eid %U marked for removal, but not found in "
 327                         "map-cache!", unformat_gid_address, deid);
 328           return VNET_API_ERROR_INVALID_VALUE;
 329         }
 330
 331       /* delete forwarding entry */
 332       del_fwd_entry (lcm, 0, mi);
 333
 334       dm_args->is_add = 0;
 335       gid_address_copy (&dm_args->deid, deid);
 336       map = pool_elt_at_index (lcm->mapping_pool, mi);
 337       dm_args->locator_set_index = map->locator_set_index;
 338
 339       /* delete mapping associated to fwd entry */
 340       vnet_lisp_add_del_mapping (dm_args, 0);
 341
 342       refc = hash_get (lcm->dp_if_refcount_by_vni, vni);
 343       ASSERT(refc != 0);
 344       refc[0]--;
 345
 346       /* remove iface if needed */
 347       if (refc[0] == 0)
 348         {
 349           ai->is_add = 0;
 350           ai->vni = vni;
 351           ai->table_id = p[0];
 352           vnet_lisp_gpe_add_del_iface (ai, 0);
 353           hash_unset (lcm->dp_if_refcount_by_vni, vni);
 354         }
 355     }
 356   return rv;
 357 }
 358
 359 /**
 360  * Adds/removes/updates static remote mapping.
 361  *
 362  * This function also modifies forwarding entries if needed.
 363  *
 364  * @param deid destination EID
 365  * @param seid source EID
 366  * @param rlocs vector of remote locators
 367  * @param action action for negative map-reply
 368  * @param is_add add mapping if non-zero, delete otherwise
 369  * @param del_all if set, delete all remote mappings
 370  * @return return code
 371  */
 372 int
 373 vnet_lisp_add_del_remote_mapping (gid_address_t * deid, gid_address_t * seid,
 374                                   ip_address_t * rlocs, u8 action, u8 is_add,
 375                                   u8 del_all)
 376 {
 377   vnet_lisp_add_del_mapping_args_t _dm_args, * dm_args = &_dm_args;
 378   vnet_lisp_add_del_mapping_args_t _sm_args, * sm_args = &_sm_args;
 379   vnet_lisp_add_del_locator_set_args_t _ls, * ls = &_ls;
 380   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
 381   u32 mi, ls_index = 0, dst_map_index, src_map_index;
 382   locator_t loc;
 383   ip_address_t * dl;
 384   int rc = -1;
 385
 386   if (del_all)
 387     return vnet_lisp_clear_all_remote_mappings ();
 388
 389   memset (sm_args, 0, sizeof (sm_args[0]));
 390   memset (dm_args, 0, sizeof (dm_args[0]));
 391   memset (ls, 0, sizeof (ls[0]));
 392
 393   /* prepare remote locator set */
 394   vec_foreach (dl, rlocs)
 395     {
 396       memset (&loc, 0, sizeof (loc));
 397       gid_address_ip (&loc.address) = dl[0];
 398       vec_add1 (ls->locators, loc);
 399     }
 400   src_map_index = gid_dictionary_lookup (&lcm->mapping_index_by_gid, seid);
 401
 402   mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, deid);
 403   /* new mapping */
 404   if ((u32)~0 == mi)
 405     {
 406       ls->is_add = 1;
 407       ls->index = ~0;
 408
 409       /* process a negative mapping */
 410       if (0 == vec_len (rlocs))
 411         return lisp_add_del_negative_static_mapping (deid, ls,
 412                                                      action, is_add);
 413
 414       if ((u32)~0 == src_map_index)
 415         {
 416           clib_warning ("seid %U not found!", format_gid_address, seid);
 417           goto done;
 418         }
 419
 420       if (!is_add)
 421         {
 422           clib_warning ("deid %U marked for removal but not "
 423                         "found!", format_gid_address, deid);
 424           goto done;
 425         }
 426
 427       vnet_lisp_add_del_locator_set (ls, &ls_index);
 428
 429       /* add mapping */
 430       gid_address_copy (&dm_args->deid, deid);
 431       dm_args->is_add = 1;
 432       dm_args->action = action;
 433       dm_args->locator_set_index = ls_index;
 434       vnet_lisp_add_del_mapping (dm_args, &dst_map_index);
 435
 436       /* add fwd tunnel */
 437       add_fwd_entry (lcm, src_map_index, dst_map_index);
 438     }
 439   else
 440     {
 441       /* delete mapping */
 442       if (!is_add)
 443         {
 444           /* delete forwarding entry */
 445           del_fwd_entry (lcm, 0, mi);
 446           dm_args->is_add = 0;
 447           gid_address_copy (&dm_args->deid, deid);
 448           mapping_t * map = pool_elt_at_index (lcm->mapping_pool, mi);
 449           dm_args->locator_set_index = map->locator_set_index;
 450
 451           /* delete mapping associated to fwd entry */
 452           vnet_lisp_add_del_mapping (dm_args, 0);
 453
 454           ls->is_add = 0;
 455           ls->index = map->locator_set_index;
 456           /* delete locator set */
 457           vnet_lisp_add_del_locator_set (ls, 0);
 458         }
 459       /* update existing locator set */
 460       else
 461         {
 462           if ((u32)~0 == src_map_index)
 463             {
 464               clib_warning ("seid %U not found!", format_gid_address, seid);
 465               goto done;
 466             }
 467
 468           mapping_t * old_map;
 469           locator_set_t * old_ls;
 470           old_map = pool_elt_at_index (lcm->mapping_pool, mi);
 471
 472           /* update mapping attributes */
 473           old_map->action = action;
 474
 475           old_ls = pool_elt_at_index(lcm->locator_set_pool,
 476                                      old_map->locator_set_index);
 477           if (compare_locators (lcm, old_ls->locator_indices,
 478                                 ls->locators))
 479             {
 480               /* set locator-set index to overwrite */
 481               ls->is_add = 1;
 482               ls->index = old_map->locator_set_index;
 483               vnet_lisp_add_del_locator_set (ls, 0);
 484               add_fwd_entry (lcm, src_map_index, mi);
 485             }
 486         }
 487     }
 488   /* success */
 489   rc = 0;
 490 done:
 491   vec_free (ls->locators);
 492   return rc;
 493 }
 494
 495 int
 496 vnet_lisp_clear_all_remote_mappings (void)
 497 {
 498   int rv = 0;
 499   u32 mi, * map_indices = 0, * map_indexp;
 500   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
 501   vnet_lisp_add_del_mapping_args_t _dm_args, * dm_args = &_dm_args;
 502   vnet_lisp_add_del_locator_set_args_t _ls, * ls = &_ls;
 503
 504   pool_foreach_index (mi, lcm->mapping_pool,
 505     ({
 506       vec_add1 (map_indices, mi);
 507     }));
 508
 509   vec_foreach (map_indexp, map_indices)
 510     {
 511       mapping_t * map = pool_elt_at_index (lcm->mapping_pool, map_indexp[0]);
 512       if (!map->local)
 513         {
 514           del_fwd_entry (lcm, 0, map_indexp[0]);
 515
 516           dm_args->is_add = 0;
 517           gid_address_copy (&dm_args->deid, &map->eid);
 518           dm_args->locator_set_index = map->locator_set_index;
 519
 520           /* delete mapping associated to fwd entry */
 521           vnet_lisp_add_del_mapping (dm_args, 0);
 522
 523           ls->is_add = 0;
 524           ls->local = 0;
 525           ls->index = map->locator_set_index;
 526           /* delete locator set */
 527           rv = vnet_lisp_add_del_locator_set (ls, 0);
 528           if (rv != 0)
 529             goto cleanup;
 530         }
 531     }
 532
 533 cleanup:
 534   if (map_indices)
 535     vec_free (map_indices);
 536   return rv;
 537 }
 538
 539 /**
 540  * Handler for add/del remote mapping CLI.
 541  *
 542  * @param vm vlib context
 543  * @param input input from user
 544  * @param cmd cmd
 545  * @return pointer to clib error structure
 546  */
 547 static clib_error_t *
 548 lisp_add_del_remote_mapping_command_fn (vlib_main_t * vm,
 549                                         unformat_input_t * input,
 550                                         vlib_cli_command_t * cmd)
 551 {
 552   clib_error_t * error = 0;
 553   unformat_input_t _line_input, * line_input = &_line_input;
 554   u8 is_add = 1, del_all = 0;
 555   ip_address_t rloc, * rlocs = 0;
 556   ip_prefix_t * deid_ippref, * seid_ippref;
 557   gid_address_t seid, deid;
 558   u8 deid_set = 0;
 559   u8 * s = 0;
 560   u32 vni, action = ~0;
 561
 562   /* Get a line of input. */
 563   if (! unformat_user (input, unformat_line_input, line_input))
 564     return 0;
 565
 566   memset (&deid, 0, sizeof (deid));
 567   memset (&seid, 0, sizeof (seid));
 568
 569   seid_ippref = &gid_address_ippref(&seid);
 570   deid_ippref = &gid_address_ippref(&deid);
 571
 572   gid_address_type (&deid) = GID_ADDR_IP_PREFIX;
 573   gid_address_type (&seid) = GID_ADDR_IP_PREFIX;
 574
 575   while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
 576     {
 577       if (unformat (line_input, "del-all"))
 578         del_all = 1;
 579       else if (unformat (line_input, "del"))
 580         is_add = 0;
 581       else if (unformat (line_input, "add"))
 582         ;
 583       else if (unformat (line_input, "deid %U",
 584                          unformat_ip_prefix, deid_ippref))
 585         {
 586           deid_set = 1;
 587         }
 588       else if (unformat (line_input, "vni %u", &vni))
 589         {
 590           gid_address_set_vni (&seid, vni);
 591           gid_address_set_vni (&deid, vni);
 592         }
 593       else if (unformat (line_input, "seid %U",
 594                          unformat_ip_prefix, seid_ippref))
 595         ;
 596       else if (unformat (line_input, "rloc %U", unformat_ip_address, &rloc))
 597         vec_add1 (rlocs, rloc);
 598       else if (unformat (line_input, "action %s", &s))
 599         {
 600           if (!strcmp ((char *)s, "no-action"))
 601             action = ACTION_NONE;
 602           if (!strcmp ((char *)s, "natively-forward"))
 603             action = ACTION_NATIVELY_FORWARDED;
 604           if (!strcmp ((char *)s, "send-map-request"))
 605             action = ACTION_SEND_MAP_REQUEST;
 606           else if (!strcmp ((char *)s, "drop"))
 607             action = ACTION_DROP;
 608           else
 609             {
 610               clib_warning ("invalid action: '%s'", s);
 611               goto done;
 612             }
 613         }
 614       else
 615         {
 616           clib_warning ("parse error");
 617           goto done;
 618         }
 619     }
 620
 621   if (!del_all)
 622     {
 623       if (!deid_set)
 624         {
 625           clib_warning ("missing deid!");
 626           goto done;
 627         }
 628
 629       if (is_add
 630           && (ip_prefix_version (deid_ippref)
 631             != ip_prefix_version (seid_ippref)))
 632         {
 633           clib_warning ("source and destination EIDs are not"
 634                         " in the same IP family!");
 635           goto done;
 636         }
 637
 638       if (is_add && (~0 == action)
 639           && 0 == vec_len (rlocs))
 640         {
 641           clib_warning ("no action set for negative map-reply!");
 642           goto done;
 643         }
 644     }
 645
 646   int rv = vnet_lisp_add_del_remote_mapping (&deid, &seid, rlocs,
 647                                              action, is_add, del_all);
 648   if (rv)
 649     clib_warning ("failed to %s remote mapping!",
 650                   is_add ? "add" : "delete");
 651
 652 done:
 653   unformat_free (line_input);
 654   if (s)
 655     vec_free (s);
 656   return error;
 657 }
 658
 659 VLIB_CLI_COMMAND (lisp_add_del_remote_mapping_command) = {
 660     .path = "lisp remote-mapping",
 661     .short_help = "lisp remote-mapping add|del [del-all] vni <vni>"
 662      "deid <dest-eid> seid <src-eid> [action <no-action|natively-forward|"
 663      "send-map-request|drop>] rloc <dst-locator> [rloc <dst-locator> ... ]",
 664     .function = lisp_add_del_remote_mapping_command_fn,
 665 };
 666
 667 static clib_error_t *
 668 lisp_show_map_resolvers_command_fn (vlib_main_t * vm,
 669                                     unformat_input_t * input,
 670                                     vlib_cli_command_t * cmd)
 671 {
 672   ip_address_t * addr;
 673   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
 674
 675   vec_foreach (addr, lcm->map_resolvers)
 676     {
 677       vlib_cli_output (vm, "%U", format_ip_address, addr);
 678     }
 679   return 0;
 680 }
 681
 682 VLIB_CLI_COMMAND (lisp_show_map_resolvers_command) = {
 683     .path = "show lisp map-resolvers",
 684     .short_help = "show lisp map-resolvers",
 685     .function = lisp_show_map_resolvers_command_fn,
 686 };
 687
 688 int
 689 vnet_lisp_pitr_set_locator_set (u8 * locator_set_name, u8 is_add)
 690 {
 691   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
 692   u32 locator_set_index = ~0;
 693   mapping_t * m;
 694   uword * p;
 695
 696   p = hash_get_mem (lcm->locator_set_index_by_name, locator_set_name);
 697   if (!p)
 698     {
 699       clib_warning ("locator-set %v doesn't exist", locator_set_name);
 700       return -1;
 701     }
 702   locator_set_index = p[0];
 703
 704   if (is_add)
 705     {
 706       pool_get (lcm->mapping_pool, m);
 707       m->locator_set_index = locator_set_index;
 708       m->local = 1;
 709       lcm->pitr_map_index = m - lcm->mapping_pool;
 710
 711       /* enable pitr mode */
 712       lcm->lisp_pitr = 1;
 713     }
 714   else
 715     {
 716       /* remove pitr mapping */
 717       pool_put_index (lcm->mapping_pool, lcm->pitr_map_index);
 718
 719       /* disable pitr mode */
 720       lcm->lisp_pitr = 0;
 721     }
 722   return 0;
 723 }
 724
 725 static clib_error_t *
 726 lisp_pitr_set_locator_set_command_fn (vlib_main_t * vm,
 727                                       unformat_input_t * input,
 728                                       vlib_cli_command_t * cmd)
 729 {
 730   u8 locator_name_set = 0;
 731   u8 * locator_set_name = 0;
 732   u8 is_add = 1;
 733   unformat_input_t _line_input, * line_input = &_line_input;
 734
 735   /* Get a line of input. */
 736   if (! unformat_user (input, unformat_line_input, line_input))
 737     return 0;
 738
 739   while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
 740     {
 741       if (unformat (line_input, "ls %_%v%_", &locator_set_name))
 742         locator_name_set = 1;
 743       else if (unformat (line_input, "disable"))
 744         is_add = 0;
 745       else
 746         return clib_error_return (0, "parse error");
 747     }
 748
 749   if (!locator_name_set)
 750     {
 751       clib_warning ("No locator set specified!");
 752       goto done;
 753     }
 754   vnet_lisp_pitr_set_locator_set (locator_set_name, is_add);
 755
 756 done:
 757   if (locator_set_name)
 758     vec_free (locator_set_name);
 759   return 0;
 760 }
 761
 762 VLIB_CLI_COMMAND (lisp_pitr_set_locator_set_command) = {
 763     .path = "lisp pitr",
 764     .short_help = "lisp pitr [disable] ls <locator-set-name>",
 765     .function = lisp_pitr_set_locator_set_command_fn,
 766 };
 767
 768 static clib_error_t *
 769 lisp_show_local_eid_table_command_fn (vlib_main_t * vm,
 770                                       unformat_input_t * input,
 771                                       vlib_cli_command_t * cmd)
 772 {
 773   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 774   mapping_t * mapit;
 775
 776   vlib_cli_output (vm, "%=20s%=16s", "EID", "Locator");
 777   pool_foreach (mapit, lcm->mapping_pool,
 778   ({
 779     u8 * msg = 0;
 780     locator_set_t * ls = pool_elt_at_index (lcm->locator_set_pool,
 781                                             mapit->locator_set_index);
 782     vlib_cli_output (vm, "%-16U%16v", format_gid_address, &mapit->eid,
 783                      ls->name);
 784     vec_free (msg);
 785   }));
 786
 787   return 0;
 788 }
 789
 790 VLIB_CLI_COMMAND (lisp_cp_show_local_eid_table_command) = {
 791     .path = "show lisp eid-table",
 792     .short_help = "Shows local EID table",
 793     .function = lisp_show_local_eid_table_command_fn,
 794 };
 795
 796 /* cleans locator to locator-set data and removes locators not part of
 797  * any locator-set */
 798 static void
 799 clean_locator_to_locator_set (lisp_cp_main_t * lcm, u32 lsi)
 800 {
 801   u32 i, j, *loc_indexp, *ls_indexp, **ls_indexes, *to_be_deleted = 0;
 802   locator_set_t * ls = pool_elt_at_index(lcm->locator_set_pool, lsi);
 803   for (i = 0; i < vec_len(ls->locator_indices); i++)
 804     {
 805       loc_indexp = vec_elt_at_index(ls->locator_indices, i);
 806       ls_indexes = vec_elt_at_index(lcm->locator_to_locator_sets,
 807                                     loc_indexp[0]);
 808       for (j = 0; j < vec_len(ls_indexes[0]); j++)
 809         {
 810           ls_indexp = vec_elt_at_index(ls_indexes[0], j);
 811           if (ls_indexp[0] == lsi)
 812             break;
 813         }
 814
 815       /* delete index for removed locator-set*/
 816       vec_del1(ls_indexes[0], j);
 817
 818       /* delete locator if it's part of no locator-set */
 819       if (vec_len (ls_indexes[0]) == 0)
 820         {
 821           pool_put_index (lcm->locator_pool, loc_indexp[0]);
 822           vec_add1 (to_be_deleted, i);
 823         }
 824     }
 825
 826   if (to_be_deleted)
 827     {
 828       for (i = 0; i < vec_len (to_be_deleted); i++)
 829         {
 830           loc_indexp = vec_elt_at_index (to_be_deleted, i);
 831           vec_del1 (ls->locator_indices, loc_indexp[0]);
 832         }
 833       vec_free (to_be_deleted);
 834     }
 835 }
 836
 837 static inline
 838 uword *get_locator_set_index(vnet_lisp_add_del_locator_set_args_t * a,
 839                              uword * p)
 840 {
 841   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 842
 843   ASSERT(a != NULL);
 844   ASSERT(p != NULL);
 845
 846   /* find locator-set */
 847   if (a->local)
 848     {
 849       p = hash_get_mem(lcm->locator_set_index_by_name, a->name);
 850     }
 851   else
 852     {
 853       *p = a->index;
 854     }
 855
 856   return p;
 857 }
 858
 859 static inline
 860 int is_locator_in_locator_set(lisp_cp_main_t * lcm, locator_set_t * ls,
 861                               locator_t * loc)
 862 {
 863   locator_t * itloc;
 864   u32 * locit;
 865
 866   ASSERT(ls != NULL);
 867   ASSERT(loc != NULL);
 868
 869   vec_foreach(locit, ls->locator_indices)
 870     {
 871       itloc = pool_elt_at_index(lcm->locator_pool, locit[0]);
 872       if (itloc->sw_if_index == loc->sw_if_index ||
 873           !gid_address_cmp(&itloc->address, &loc->address))
 874         {
 875           clib_warning("Duplicate locator");
 876           return VNET_API_ERROR_VALUE_EXIST;
 877         }
 878     }
 879
 880   return 0;
 881 }
 882
 883 static inline
 884 void remove_locator_from_locator_set(locator_set_t * ls, u32 * locit,
 885                                      u32 ls_index, u32 loc_id)
 886 {
 887   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 888   u32 ** ls_indexes = NULL;
 889
 890   ASSERT(ls != NULL);
 891   ASSERT(locit != NULL);
 892
 893   ls_indexes = vec_elt_at_index(lcm->locator_to_locator_sets,
 894                                 locit[0]);
 895   pool_put_index(lcm->locator_pool, locit[0]);
 896   vec_del1(ls->locator_indices, loc_id);
 897   vec_del1(ls_indexes[0], ls_index);
 898 }
 899
 900 int
 901 vnet_lisp_add_del_locator (vnet_lisp_add_del_locator_set_args_t * a,
 902                            locator_set_t * ls, u32 * ls_result)
 903 {
 904   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 905   locator_t * loc = NULL, *itloc = NULL;
 906   uword _p = (u32)~0, * p = &_p;
 907   u32 loc_index = ~0, ls_index = ~0, * locit = NULL, ** ls_indexes = NULL;
 908   u32 loc_id = ~0;
 909   int ret = 0;
 910
 911   ASSERT(a != NULL);
 912
 913   p = get_locator_set_index(a, p);
 914   if (!p)
 915     {
 916       clib_warning("locator-set %v doesn't exist", a->name);
 917       return VNET_API_ERROR_INVALID_ARGUMENT;
 918     }
 919
 920   if (ls == 0)
 921     {
 922       ls = pool_elt_at_index(lcm->locator_set_pool, p[0]);
 923       if (!ls)
 924         {
 925           clib_warning("locator-set %d to be overwritten doesn't exist!",
 926                        p[0]);
 927           return VNET_API_ERROR_INVALID_ARGUMENT;
 928         }
 929     }
 930
 931   if (a->is_add)
 932     {
 933
 934         if (ls_result)
 935           ls_result[0] = p[0];
 936
 937         /* allocate locators */
 938         vec_foreach (itloc, a->locators)
 939           {
 940             ret = is_locator_in_locator_set(lcm, ls, itloc);
 941             if (0 != ret)
 942               {
 943                 return ret;
 944               }
 945
 946             pool_get(lcm->locator_pool, loc);
 947             loc[0] = itloc[0];
 948             loc_index = loc - lcm->locator_pool;
 949
 950             vec_add1(ls->locator_indices, loc_index);
 951
 952             vec_validate (lcm->locator_to_locator_sets, loc_index);
 953             ls_indexes = vec_elt_at_index(lcm->locator_to_locator_sets,
 954                                           loc_index);
 955             vec_add1(ls_indexes[0], ls_index);
 956           }
 957       }
 958     else
 959       {
 960         ls_index = p[0];
 961
 962         itloc = a->locators;
 963         loc_id = 0;
 964         vec_foreach (locit, ls->locator_indices)
 965           {
 966             loc = pool_elt_at_index(lcm->locator_pool, locit[0]);
 967
 968             if (loc->local && loc->sw_if_index == itloc->sw_if_index)
 969               {
 970                 remove_locator_from_locator_set(ls, locit,
 971                                                 ls_index, loc_id);
 972               }
 973             if (0 == loc->local &&
 974                 !gid_address_cmp(&loc->address, &itloc->address))
 975               {
 976                 remove_locator_from_locator_set(ls, locit,
 977                                                 ls_index, loc_id);
 978               }
 979
 980             loc_id++;
 981           }
 982       }
 983
 984   return 0;
 985 }
 986
 987 int
 988 vnet_lisp_add_del_locator_set (vnet_lisp_add_del_locator_set_args_t * a,
 989                                u32 * ls_result)
 990 {
 991   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 992   locator_set_t * ls;
 993   uword _p = (u32)~0, * p = &_p;
 994   u32 ls_index;
 995   u32 ** eid_indexes;
 996   int ret = 0;
 997
 998   if (a->is_add)
 999     {
1000       p = get_locator_set_index(a, p);
1001
1002       /* overwrite */
1003       if (p && p[0] != (u32)~0)
1004         {
1005           ls = pool_elt_at_index(lcm->locator_set_pool, p[0]);
1006           if (!ls)
1007             {
1008               clib_warning("locator-set %d to be overwritten doesn't exist!",
1009                            p[0]);
1010               return -1;
1011             }
1012
1013           /* clean locator to locator-set vectors and remove locators if
1014            * they're not part of another locator-set */
1015           clean_locator_to_locator_set (lcm, p[0]);
1016
1017           /* remove locator indices from locator set */
1018           vec_free(ls->locator_indices);
1019
1020           ls_index = p[0];
1021
1022           if (ls_result)
1023             ls_result[0] = p[0];
1024         }
1025       /* new locator-set */
1026       else
1027         {
1028           pool_get(lcm->locator_set_pool, ls);
1029           memset(ls, 0, sizeof(*ls));
1030           ls_index = ls - lcm->locator_set_pool;
1031
1032           if (a->local)
1033             {
1034               ls->name = vec_dup(a->name);
1035
1036               if (!lcm->locator_set_index_by_name)
1037                 lcm->locator_set_index_by_name = hash_create_vec(
1038                     /* size */0, sizeof(ls->name[0]), sizeof(uword));
1039               hash_set_mem(lcm->locator_set_index_by_name, ls->name, ls_index);
1040
1041               /* mark as local locator-set */
1042               vec_add1(lcm->local_locator_set_indexes, ls_index);
1043             }
1044           ls->local = a->local;
1045           if (ls_result)
1046             ls_result[0] = ls_index;
1047         }
1048
1049       ret = vnet_lisp_add_del_locator(a, ls, NULL);
1050       if (0 != ret)
1051         {
1052           return ret;
1053         }
1054     }
1055   else
1056     {
1057       p = get_locator_set_index(a, p);
1058       if (!p)
1059         {
1060           clib_warning("locator-set %v doesn't exists", a->name);
1061           return -1;
1062         }
1063
1064       ls = pool_elt_at_index(lcm->locator_set_pool, p[0]);
1065       if (!ls)
1066         {
1067           clib_warning("locator-set with index %d doesn't exists", p[0]);
1068           return -1;
1069         }
1070
1071       if (vec_len(lcm->locator_set_to_eids) != 0)
1072       {
1073           eid_indexes = vec_elt_at_index(lcm->locator_set_to_eids, p[0]);
1074           if (vec_len(eid_indexes[0]) != 0)
1075           {
1076               clib_warning ("Can't delete a locator that supports a mapping!");
1077               return -1;
1078           }
1079       }
1080
1081       /* clean locator to locator-sets data */
1082       clean_locator_to_locator_set (lcm, p[0]);
1083
1084       if (ls->local)
1085         {
1086           u32 it, lsi;
1087
1088           vec_foreach_index(it, lcm->local_locator_set_indexes)
1089           {
1090             lsi = vec_elt(lcm->local_locator_set_indexes, it);
1091             if (lsi == p[0])
1092               {
1093                 vec_del1(lcm->local_locator_set_indexes, it);
1094                 break;
1095               }
1096           }
1097           hash_unset_mem(lcm->locator_set_index_by_name, ls->name);
1098         }
1099       vec_free(ls->name);
1100       vec_free(ls->locator_indices);
1101       pool_put(lcm->locator_set_pool, ls);
1102     }
1103   return 0;
1104 }
1105
1106 clib_error_t *
1107 vnet_lisp_enable_disable (u8 is_enabled)
1108 {
1109   vnet_lisp_gpe_add_del_iface_args_t _ai, * ai= &_ai;
1110   uword * table_id, * refc;
1111   u32 i;
1112   clib_error_t * error = 0;
1113   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
1114   vnet_lisp_gpe_enable_disable_args_t _a, * a = &_a;
1115
1116   a->is_en = is_enabled;
1117   error = vnet_lisp_gpe_enable_disable (a);
1118   if (error)
1119     {
1120       return clib_error_return (0, "failed to %s data-plane!",
1121                                 a->is_en ? "enable" : "disable");
1122     }
1123
1124   if (is_enabled)
1125     {
1126       /* enable all ifaces */
1127       for (i = 0; i < vec_len (lcm->local_mappings_indexes); i++)
1128         {
1129           mapping_t * m = vec_elt_at_index (lcm->mapping_pool, i);
1130           ai->is_add = 1;
1131           ai->vni = gid_address_vni (&m->eid);
1132
1133           refc = hash_get (lcm->dp_if_refcount_by_vni, ai->vni);
1134           if (!refc)
1135             {
1136               table_id = hash_get (lcm->table_id_by_vni, ai->vni);
1137               if (table_id)
1138                 {
1139                   ai->table_id = table_id[0];
1140                   /* enables interface and adds defaults */
1141                   vnet_lisp_gpe_add_del_iface (ai, 0);
1142                 }
1143               else
1144                 return clib_error_return (0, "no table_id found for vni %u!",
1145                                           ai->vni);
1146
1147               hash_set (lcm->dp_if_refcount_by_vni, ai->vni, 1);
1148             }
1149           else
1150             {
1151               refc[0]++;
1152             }
1153         }
1154     }
1155   else
1156     {
1157       /* clear refcount table */
1158       hash_free (lcm->dp_if_refcount_by_vni);
1159       hash_free (lcm->fwd_entry_by_mapping_index);
1160       pool_free (lcm->fwd_entry_pool);
1161     }
1162
1163   /* update global flag */
1164   lcm->is_enabled = is_enabled;
1165
1166   return 0;
1167 }
1168
1169 static clib_error_t *
1170 lisp_enable_disable_command_fn (vlib_main_t * vm, unformat_input_t * input,
1171                                    vlib_cli_command_t * cmd)
1172 {
1173   unformat_input_t _line_input, * line_input = &_line_input;
1174   u8 is_enabled = 0;
1175   u8 is_set = 0;
1176
1177   /* Get a line of input. */
1178   if (! unformat_user (input, unformat_line_input, line_input))
1179     return 0;
1180
1181   while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
1182     {
1183       if (unformat (line_input, "enable"))
1184         {
1185           is_set = 1;
1186           is_enabled = 1;
1187         }
1188       else if (unformat (line_input, "disable"))
1189         is_set = 1;
1190       else
1191         {
1192           return clib_error_return (0, "parse error: '%U'",
1193                                    format_unformat_error, line_input);
1194         }
1195     }
1196
1197   if (!is_set)
1198       return clib_error_return (0, "state not set");
1199
1200   return vnet_lisp_enable_disable (is_enabled);
1201 }
1202
1203 VLIB_CLI_COMMAND (lisp_cp_enable_disable_command) = {
1204     .path = "lisp",
1205     .short_help = "lisp [enable|disable]",
1206     .function = lisp_enable_disable_command_fn,
1207 };
1208
1209 u8
1210 vnet_lisp_enable_disable_status (void)
1211 {
1212   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
1213   return lcm->is_enabled;
1214 }
1215
1216 static u8 *
1217 format_lisp_status (u8 * s, va_list * args)
1218 {
1219   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
1220   return format (s, "%s", lcm->is_enabled ? "enabled" : "disabled");
1221 }
1222
1223 static clib_error_t *
1224 lisp_show_status_command_fn (vlib_main_t * vm, unformat_input_t * input,
1225                              vlib_cli_command_t * cmd)
1226 {
1227   u8 * msg = 0;
1228   msg = format (msg, "feature: %U\ngpe: %U\n",
1229                 format_lisp_status, format_vnet_lisp_gpe_status);
1230   vlib_cli_output (vm, "%v", msg);
1231   vec_free (msg);
1232   return 0;
1233 }
1234
1235 VLIB_CLI_COMMAND (lisp_show_status_command) = {
1236     .path = "show lisp status",
1237     .short_help = "show lisp status",
1238     .function = lisp_show_status_command_fn,
1239 };
1240 static clib_error_t *
1241 lisp_add_del_locator_set_command_fn (vlib_main_t * vm, unformat_input_t * input,
1242                                      vlib_cli_command_t * cmd)
1243 {
1244   lisp_gpe_main_t * lgm = &lisp_gpe_main;
1245   vnet_main_t * vnm = lgm->vnet_main;
1246   unformat_input_t _line_input, * line_input = &_line_input;
1247   u8 is_add = 1;
1248   clib_error_t * error = 0;
1249   u8 * locator_set_name = 0;
1250   locator_t locator, * locators = 0;
1251   vnet_lisp_add_del_locator_set_args_t _a, * a = &_a;
1252   u32 ls_index = 0;
1253
1254   memset(&locator, 0, sizeof(locator));
1255   memset(a, 0, sizeof(a[0]));
1256
1257   /* Get a line of input. */
1258   if (! unformat_user (input, unformat_line_input, line_input))
1259     return 0;
1260
1261   while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
1262     {
1263       if (unformat (line_input, "add %_%v%_", &locator_set_name))
1264         is_add = 1;
1265       else if (unformat (line_input, "del %_%v%_", &locator_set_name))
1266         is_add = 0;
1267       else if (unformat (line_input, "iface %U p %d w %d",
1268                          unformat_vnet_sw_interface, vnm, &locator.sw_if_index,
1269                          &locator.priority, &locator.weight))
1270         {
1271           locator.local = 1;
1272           vec_add1(locators, locator);
1273         }
1274       else
1275         {
1276           error = unformat_parse_error(line_input);
1277           goto done;
1278         }
1279     }
1280
1281   a->name = locator_set_name;
1282   a->locators = locators;
1283   a->is_add = is_add;
1284   a->local = 1;
1285
1286   vnet_lisp_add_del_locator_set(a, &ls_index);
1287
1288  done:
1289   vec_free(locators);
1290   if (locator_set_name)
1291     vec_free (locator_set_name);
1292   return error;
1293 }
1294
1295 VLIB_CLI_COMMAND (lisp_cp_add_del_locator_set_command) = {
1296     .path = "lisp locator-set",
1297     .short_help = "lisp locator-set add/del <name> iface <iface-name> "
1298         "p <priority> w <weight>",
1299     .function = lisp_add_del_locator_set_command_fn,
1300 };
1301
1302 static clib_error_t *
1303 lisp_cp_show_locator_sets_command_fn (vlib_main_t * vm,
1304                                       unformat_input_t * input,
1305                                       vlib_cli_command_t * cmd)
1306 {
1307   locator_set_t * lsit;
1308   locator_t * loc;
1309   u32 * locit;
1310   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
1311
1312   vlib_cli_output (vm, "%=20s%=16s%=16s%=16s", "Locator-set", "Locator",
1313                    "Priority", "Weight");
1314   pool_foreach (lsit, lcm->locator_set_pool,
1315   ({
1316     u8 * msg = 0;
1317     msg = format (msg, "%-16v", lsit->name);
1318     vec_foreach (locit, lsit->locator_indices)
1319       {
1320         loc = pool_elt_at_index (lcm->locator_pool, locit[0]);
1321         if (loc->local)
1322           msg = format (msg, "%16d%16d%16d", loc->sw_if_index, loc->priority,
1323                         loc->weight);
1324         else
1325           msg = format (msg, "%16U%16d%16d", format_gid_address, &loc->address,
1326                         loc->priority, loc->weight);
1327       }
1328     vlib_cli_output (vm, "%v", msg);
1329     vec_free (msg);
1330   }));
1331   return 0;
1332 }
1333
1334 VLIB_CLI_COMMAND (lisp_cp_show_locator_sets_command) = {
1335     .path = "show lisp locator-set",
1336     .short_help = "Shows locator-sets",
1337     .function = lisp_cp_show_locator_sets_command_fn,
1338 };
1339
1340 int
1341 vnet_lisp_add_del_map_resolver (vnet_lisp_add_del_map_resolver_args_t * a)
1342 {
1343   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
1344   ip_address_t * addr;
1345   u32 i;
1346
1347   if (a->is_add)
1348     {
1349       vec_foreach(addr, lcm->map_resolvers)
1350         {
1351           if (!ip_address_cmp (addr, &a->address))
1352             {
1353               clib_warning("map-resolver %U already exists!", format_ip_address,
1354                            &a->address);
1355               return -1;
1356             }
1357         }
1358       vec_add1(lcm->map_resolvers, a->address);
1359     }
1360   else
1361     {
1362       for (i = 0; i < vec_len(lcm->map_resolvers); i++)
1363         {
1364           addr = vec_elt_at_index(lcm->map_resolvers, i);
1365           if (!ip_address_cmp (addr, &a->address))
1366             {
1367               vec_delete(lcm->map_resolvers, 1, i);
1368               break;
1369             }
1370         }
1371     }
1372   return 0;
1373 }
1374
1375 static clib_error_t *
1376 lisp_add_del_map_resolver_command_fn (vlib_main_t * vm,
1377                                          unformat_input_t * input,
1378                                          vlib_cli_command_t * cmd)
1379 {
1380   unformat_input_t _line_input, * line_input = &_line_input;
1381   u8 is_add = 1;
1382   ip_address_t ip_addr;
1383   clib_error_t * error = 0;
1384   vnet_lisp_add_del_map_resolver_args_t _a, * a = &_a;
1385
1386   /* Get a line of input. */
1387   if (! unformat_user (input, unformat_line_input, line_input))
1388     return 0;
1389
1390   while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
1391     {
1392       if (unformat (line_input, "add"))
1393         is_add = 1;
1394       else if (unformat (line_input, "del"))
1395         is_add = 0;
1396       else if (unformat (line_input, "%U", unformat_ip_address, &ip_addr))
1397         ;
1398       else
1399         {
1400           error = unformat_parse_error(line_input);
1401           goto done;
1402         }
1403     }
1404   a->is_add = is_add;
1405   a->address = ip_addr;
1406   vnet_lisp_add_del_map_resolver (a);
1407
1408  done:
1409   return error;
1410 }
1411
1412 VLIB_CLI_COMMAND (lisp_add_del_map_resolver_command) = {
1413     .path = "lisp map-resolver",
1414     .short_help = "lisp map-resolver add/del <ip_address>",
1415     .function = lisp_add_del_map_resolver_command_fn,
1416 };
1417
1418 /* Statistics (not really errors) */
1419 #define foreach_lisp_cp_lookup_error           \
1420 _(DROP, "drop")                                \
1421 _(MAP_REQUESTS_SENT, "map-request sent")
1422
1423 static char * lisp_cp_lookup_error_strings[] = {
1424 #define _(sym,string) string,
1425   foreach_lisp_cp_lookup_error
1426 #undef _
1427 };
1428
1429 typedef enum
1430 {
1431 #define _(sym,str) LISP_CP_LOOKUP_ERROR_##sym,
1432     foreach_lisp_cp_lookup_error
1433 #undef _
1434     LISP_CP_LOOKUP_N_ERROR,
1435 } lisp_cp_lookup_error_t;
1436
1437 typedef enum
1438 {
1439   LISP_CP_LOOKUP_NEXT_DROP,
1440   LISP_CP_LOOKUP_NEXT_IP4_LOOKUP,
1441   LISP_CP_LOOKUP_NEXT_IP6_LOOKUP,
1442   LISP_CP_LOOKUP_N_NEXT,
1443 } lisp_cp_lookup_next_t;
1444
1445 typedef struct
1446 {
1447   gid_address_t dst_eid;
1448   ip_address_t map_resolver_ip;
1449 } lisp_cp_lookup_trace_t;
1450
1451 u8 *
1452 format_lisp_cp_lookup_trace (u8 * s, va_list * args)
1453 {
1454   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
1455   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
1456   lisp_cp_lookup_trace_t * t = va_arg (*args, lisp_cp_lookup_trace_t *);
1457
1458   s = format (s, "LISP-CP-LOOKUP: map-resolver: %U destination eid %U",
1459               format_ip_address, &t->map_resolver_ip, format_gid_address,
1460               &t->dst_eid);
1461   return s;
1462 }
1463
1464 static u32
1465 ip_fib_lookup_with_table (lisp_cp_main_t * lcm, u32 fib_index,
1466                           ip_address_t * dst)
1467 {
1468   if (ip_addr_version (dst) == IP4)
1469       return ip4_fib_lookup_with_table (lcm->im4, fib_index, &ip_addr_v4(dst),
1470                                         0);
1471   else
1472       return ip6_fib_lookup_with_table (lcm->im6, fib_index, &ip_addr_v6(dst));
1473 }
1474
1475 void
1476 get_mr_and_local_iface_ip (lisp_cp_main_t *lcm, ip_address_t * mr_ip,
1477                             ip_address_t * sloc)
1478 {
1479   u32 adj_index;
1480   ip_adjacency_t * adj;
1481   ip_interface_address_t * ia = 0;
1482   ip_lookup_main_t * lm;
1483   ip4_address_t * l4 = 0;
1484   ip6_address_t * l6 = 0;
1485   ip_address_t * mrit;
1486
1487   if (vec_len(lcm->map_resolvers) == 0)
1488     {
1489       clib_warning("No map-resolver configured");
1490       return;
1491     }
1492
1493   /* find the first mr ip we have a route to and the ip of the
1494    * iface that has a route to it */
1495   vec_foreach(mrit, lcm->map_resolvers)
1496     {
1497       lm = ip_addr_version (mrit) == IP4 ?
1498           &lcm->im4->lookup_main : &lcm->im6->lookup_main;
1499
1500       adj_index = ip_fib_lookup_with_table (lcm, 0, mrit);
1501       adj = ip_get_adjacency (lm, adj_index);
1502
1503       if (adj == 0)
1504         continue;
1505
1506       if (adj->lookup_next_index == IP_LOOKUP_NEXT_ARP)
1507         {
1508           ia = pool_elt_at_index(lm->if_address_pool, adj->if_address_index);
1509           if (ip_addr_version(mrit) == IP4)
1510             {
1511               l4 = ip_interface_address_get_address (lm, ia);
1512             }
1513           else
1514             {
1515               l6 = ip_interface_address_get_address (lm, ia);
1516             }
1517         }
1518       else if (adj->lookup_next_index == IP_LOOKUP_NEXT_REWRITE)
1519         {
1520           /* find sw_if_index in rewrite header */
1521           u32 sw_if_index = adj->rewrite_header.sw_if_index;
1522
1523           /* find suitable address */
1524           if (ip_addr_version(mrit) == IP4)
1525             {
1526               /* find the first ip address */
1527               foreach_ip_interface_address (&lcm->im4->lookup_main, ia,
1528                                             sw_if_index, 1 /* unnumbered */,
1529               ({
1530                 l4 = ip_interface_address_get_address (&lcm->im4->lookup_main,
1531                                                        ia);
1532                 break;
1533               }));
1534             }
1535           else
1536             {
1537               /* find the first ip address */
1538               foreach_ip_interface_address (&lcm->im6->lookup_main, ia,
1539                                             sw_if_index, 1 /* unnumbered */,
1540               ({
1541                 l6 = ip_interface_address_get_address (&lcm->im6->lookup_main,
1542                                                        ia);
1543                 break;
1544               }));
1545             }
1546         }
1547
1548       if (l4)
1549         {
1550           ip_addr_v4(sloc).as_u32 = l4->as_u32;
1551           ip_addr_version(sloc) = IP4;
1552           ip_address_copy(mr_ip, mrit);
1553           return;
1554         }
1555       else if (l6)
1556         {
1557           clib_memcpy (&ip_addr_v6(sloc), l6, sizeof(*l6));
1558           ip_addr_version(sloc) = IP6;
1559           ip_address_copy(mr_ip, mrit);
1560           return;
1561         }
1562     }
1563
1564   clib_warning("Can't find map-resolver and local interface ip!");
1565   return;
1566 }
1567
1568 static gid_address_t *
1569 build_itr_rloc_list (lisp_cp_main_t * lcm, locator_set_t * loc_set)
1570 {
1571   ip4_address_t * l4;
1572   ip6_address_t * l6;
1573   u32 i;
1574   locator_t * loc;
1575   u32 * loc_indexp;
1576   ip_interface_address_t * ia = 0;
1577   gid_address_t gid_data, * gid = &gid_data;
1578   gid_address_t * rlocs = 0;
1579   ip_prefix_t * ippref = &gid_address_ippref (gid);
1580   ip_address_t * rloc = &ip_prefix_addr (ippref);
1581
1582   gid_address_type (gid) = GID_ADDR_IP_PREFIX;
1583   for (i = 0; i < vec_len(loc_set->locator_indices); i++)
1584     {
1585       loc_indexp = vec_elt_at_index(loc_set->locator_indices, i);
1586       loc = pool_elt_at_index (lcm->locator_pool, loc_indexp[0]);
1587
1588       ip_addr_version(rloc) = IP4;
1589       /* Add ipv4 locators first TODO sort them */
1590       foreach_ip_interface_address (&lcm->im4->lookup_main, ia,
1591                                     loc->sw_if_index, 1 /* unnumbered */,
1592       ({
1593         l4 = ip_interface_address_get_address (&lcm->im4->lookup_main, ia);
1594         ip_addr_v4 (rloc) = l4[0];
1595         ip_prefix_len (ippref) = 32;
1596         vec_add1 (rlocs, gid[0]);
1597       }));
1598
1599       ip_addr_version(rloc) = IP6;
1600       /* Add ipv6 locators */
1601       foreach_ip_interface_address (&lcm->im6->lookup_main, ia,
1602                                     loc->sw_if_index, 1 /* unnumbered */,
1603       ({
1604         l6 = ip_interface_address_get_address (&lcm->im6->lookup_main, ia);
1605         ip_addr_v6 (rloc) = l6[0];
1606         ip_prefix_len (ippref) = 128;
1607         vec_add1 (rlocs, gid[0]);
1608       }));
1609     }
1610   return rlocs;
1611 }
1612
1613 static vlib_buffer_t *
1614 build_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
1615                                 gid_address_t * seid, gid_address_t * deid,
1616                                 locator_set_t * loc_set, ip_address_t * mr_ip,
1617                                 ip_address_t * sloc, u8 is_smr_invoked,
1618                                 u64 *nonce_res, u32 * bi_res)
1619 {
1620   vlib_buffer_t * b;
1621   u32 bi;
1622   gid_address_t * rlocs = 0;
1623
1624   if (vlib_buffer_alloc (vm, &bi, 1) != 1)
1625     {
1626       clib_warning ("Can't allocate buffer for Map-Request!");
1627       return 0;
1628     }
1629
1630   b = vlib_get_buffer (vm, bi);
1631
1632   /* leave some space for the encap headers */
1633   vlib_buffer_make_headroom (b, MAX_LISP_MSG_ENCAP_LEN);
1634
1635   /* get rlocs */
1636   rlocs = build_itr_rloc_list (lcm, loc_set);
1637
1638   /* put lisp msg */
1639   lisp_msg_put_mreq (lcm, b, seid, deid, rlocs, is_smr_invoked, nonce_res);
1640
1641   /* push ecm: udp-ip-lisp */
1642   lisp_msg_push_ecm (vm, b, LISP_CONTROL_PORT, LISP_CONTROL_PORT, seid, deid);
1643
1644   /* push outer ip header */
1645   pkt_push_udp_and_ip (vm, b, LISP_CONTROL_PORT, LISP_CONTROL_PORT, sloc,
1646                        mr_ip);
1647
1648   bi_res[0] = bi;
1649
1650   if (rlocs)
1651     vec_free(rlocs);
1652   return b;
1653 }
1654
1655 static void
1656 send_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
1657                                gid_address_t * seid, gid_address_t * deid,
1658                                u8 is_smr_invoked)
1659 {
1660   u32 next_index, bi = 0, * to_next, map_index;
1661   vlib_buffer_t * b;
1662   vlib_frame_t * f;
1663   u64 nonce = 0;
1664   locator_set_t * loc_set;
1665   mapping_t * map;
1666   pending_map_request_t * pmr;
1667   ip_address_t mr_ip, sloc;
1668
1669   /* get locator-set for seid */
1670   if (!lcm->lisp_pitr)
1671     {
1672       map_index = gid_dictionary_lookup (&lcm->mapping_index_by_gid, seid);
1673       if (map_index == ~0)
1674         {
1675           clib_warning("No local mapping found in eid-table for %U!",
1676                        format_gid_address, seid);
1677           return;
1678         }
1679
1680       map = pool_elt_at_index (lcm->mapping_pool, map_index);
1681
1682       if (!map->local)
1683         {
1684           clib_warning("Mapping found for src eid %U is not marked as local!",
1685                        format_gid_address, seid);
1686           return;
1687         }
1688     }
1689   else
1690     {
1691       map_index = lcm->pitr_map_index;
1692       map = pool_elt_at_index (lcm->mapping_pool, lcm->pitr_map_index);
1693     }
1694
1695   loc_set = pool_elt_at_index (lcm->locator_set_pool, map->locator_set_index);
1696
1697   /* get local iface ip to use in map-request XXX fib 0 for now*/
1698   get_mr_and_local_iface_ip (lcm, &mr_ip, &sloc);
1699
1700   /* build the encapsulated map request */
1701   b = build_encapsulated_map_request (vm, lcm, seid, deid, loc_set, &mr_ip,
1702                                       &sloc, is_smr_invoked, &nonce, &bi);
1703
1704   if (!b)
1705     return;
1706
1707   /* set fib index and lookup node */
1708   vnet_buffer(b)->sw_if_index[VLIB_TX] = ~0;
1709   next_index = (ip_addr_version(&mr_ip) == IP4) ?
1710       ip4_lookup_node.index : ip6_lookup_node.index;
1711
1712   f = vlib_get_frame_to_node (vm, next_index);
1713
1714   /* Enqueue the packet */
1715   to_next = vlib_frame_vector_args (f);
1716   to_next[0] = bi;
1717   f->n_vectors = 1;
1718   vlib_put_frame_to_node (vm, next_index, f);
1719
1720   /* add map-request to pending requests table */
1721   pool_get(lcm->pending_map_requests_pool, pmr);
1722   gid_address_copy (&pmr->src, seid);
1723   gid_address_copy (&pmr->dst, deid);
1724   pmr->src_mapping_index = map_index;
1725   hash_set(lcm->pending_map_requests_by_nonce, nonce,
1726            pmr - lcm->pending_map_requests_pool);
1727 }
1728
1729 static void
1730 get_src_and_dst (void *hdr, ip_address_t * src, ip_address_t *dst)
1731 {
1732   ip4_header_t * ip4 = hdr;
1733   ip6_header_t * ip6;
1734
1735   if ((ip4->ip_version_and_header_length & 0xF0) == 0x40)
1736     {
1737       ip_addr_v4(src).as_u32 = ip4->src_address.as_u32;
1738       ip_addr_version(src) = IP4;
1739       ip_addr_v4(dst).as_u32 = ip4->dst_address.as_u32;
1740       ip_addr_version(dst) = IP4;
1741     }
1742   else
1743     {
1744       ip6 = hdr;
1745       clib_memcpy (&ip_addr_v6(src), &ip6->src_address, sizeof(ip6->src_address));
1746       ip_addr_version(src) = IP6;
1747       clib_memcpy (&ip_addr_v6(dst), &ip6->dst_address, sizeof(ip6->dst_address));
1748       ip_addr_version(dst) = IP6;
1749     }
1750 }
1751
1752 static uword
1753 lisp_cp_lookup (vlib_main_t * vm, vlib_node_runtime_t * node,
1754               vlib_frame_t * from_frame)
1755 {
1756   u32 * from, * to_next_drop, di, si;
1757   lisp_cp_main_t *lcm = vnet_lisp_cp_get_main();
1758   u32 pkts_mapped = 0;
1759   uword n_left_from, n_left_to_next_drop;
1760
1761   from = vlib_frame_vector_args (from_frame);
1762   n_left_from = from_frame->n_vectors;
1763
1764   while (n_left_from > 0)
1765     {
1766       vlib_get_next_frame (vm, node, LISP_CP_LOOKUP_NEXT_DROP,
1767                            to_next_drop, n_left_to_next_drop);
1768
1769       while (n_left_from > 0 && n_left_to_next_drop > 0)
1770         {
1771           u32 pi0;
1772           vlib_buffer_t * p0;
1773           ip4_header_t * ip0;
1774           gid_address_t src, dst;
1775           ip_prefix_t * spref, * dpref;
1776
1777           gid_address_type (&src) = GID_ADDR_IP_PREFIX;
1778           spref = &gid_address_ippref(&src);
1779           gid_address_type (&dst) = GID_ADDR_IP_PREFIX;
1780           dpref = &gid_address_ippref(&dst);
1781
1782           pi0 = from[0];
1783           from += 1;
1784           n_left_from -= 1;
1785           to_next_drop[0] = pi0;
1786           to_next_drop += 1;
1787           n_left_to_next_drop -= 1;
1788
1789           p0 = vlib_get_buffer (vm, pi0);
1790           p0->error = node->errors[LISP_CP_LOOKUP_ERROR_DROP];
1791
1792           /* src/dst eid pair */
1793           ip0 = vlib_buffer_get_current (p0);
1794           get_src_and_dst (ip0, &ip_prefix_addr(spref), &ip_prefix_addr(dpref));
1795           ip_prefix_len(spref) = ip_address_max_len (ip_prefix_version(spref));
1796           ip_prefix_len(dpref) = ip_address_max_len (ip_prefix_version(dpref));
1797
1798           /* if we have remote mapping for destination already in map-chache
1799              add forwarding tunnel directly. If not send a map-request */
1800           di = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &dst);
1801           if (~0 != di)
1802             {
1803               mapping_t * m =  vec_elt_at_index (lcm->mapping_pool, di);
1804               /* send a map-request also in case of negative mapping entry
1805                 with corresponding action */
1806               if (m->action == ACTION_SEND_MAP_REQUEST)
1807                 {
1808                   /* send map-request */
1809                   send_encapsulated_map_request (vm, lcm, &src, &dst, 0);
1810                   pkts_mapped++;
1811                 }
1812               else
1813                 {
1814                   si =  gid_dictionary_lookup (&lcm->mapping_index_by_gid,
1815                                                &src);
1816                   if (~0 != si)
1817                     {
1818                       add_fwd_entry (lcm, si, di);
1819                     }
1820                 }
1821             }
1822           else
1823             {
1824               /* send map-request */
1825               send_encapsulated_map_request (vm, lcm, &src, &dst, 0);
1826               pkts_mapped++;
1827             }
1828
1829           if (PREDICT_FALSE(p0->flags & VLIB_BUFFER_IS_TRACED))
1830             {
1831               lisp_cp_lookup_trace_t *tr = vlib_add_trace (vm, node, p0,
1832                                                           sizeof(*tr));
1833
1834               memset(tr, 0, sizeof(*tr));
1835               gid_address_copy (&tr->dst_eid, &dst);
1836               if (vec_len(lcm->map_resolvers) > 0)
1837                 {
1838                   clib_memcpy (&tr->map_resolver_ip,
1839                                vec_elt_at_index(lcm->map_resolvers, 0),
1840                                sizeof(ip_address_t));
1841                 }
1842             }
1843         }
1844
1845       vlib_put_next_frame (vm, node, LISP_CP_LOOKUP_NEXT_DROP, n_left_to_next_drop);
1846     }
1847   vlib_node_increment_counter (vm, node->node_index,
1848                                LISP_CP_LOOKUP_ERROR_MAP_REQUESTS_SENT,
1849                                pkts_mapped);
1850   return from_frame->n_vectors;
1851 }
1852
1853 VLIB_REGISTER_NODE (lisp_cp_lookup_node) = {
1854   .function = lisp_cp_lookup,
1855   .name = "lisp-cp-lookup",
1856   .vector_size = sizeof (u32),
1857   .format_trace = format_lisp_cp_lookup_trace,
1858   .type = VLIB_NODE_TYPE_INTERNAL,
1859
1860   .n_errors = LISP_CP_LOOKUP_N_ERROR,
1861   .error_strings = lisp_cp_lookup_error_strings,
1862
1863   .n_next_nodes = LISP_CP_LOOKUP_N_NEXT,
1864
1865   .next_nodes = {
1866       [LISP_CP_LOOKUP_NEXT_DROP] = "error-drop",
1867       [LISP_CP_LOOKUP_NEXT_IP4_LOOKUP] = "ip4-lookup",
1868       [LISP_CP_LOOKUP_NEXT_IP6_LOOKUP] = "ip6-lookup",
1869   },
1870 };
1871
1872 /* lisp_cp_input statistics */
1873 #define foreach_lisp_cp_input_error                   \
1874 _(DROP, "drop")                                        \
1875 _(MAP_REPLIES_RECEIVED, "map-replies received")
1876
1877 static char * lisp_cp_input_error_strings[] = {
1878 #define _(sym,string) string,
1879   foreach_lisp_cp_input_error
1880 #undef _
1881 };
1882
1883 typedef enum
1884 {
1885 #define _(sym,str) LISP_CP_INPUT_ERROR_##sym,
1886     foreach_lisp_cp_input_error
1887 #undef _
1888     LISP_CP_INPUT_N_ERROR,
1889 } lisp_cp_input_error_t;
1890
1891 typedef enum
1892 {
1893   LISP_CP_INPUT_NEXT_DROP,
1894   LISP_CP_INPUT_N_NEXT,
1895 } lisp_cp_input_next_t;
1896
1897 typedef struct
1898 {
1899   gid_address_t dst_eid;
1900   ip4_address_t map_resolver_ip;
1901 } lisp_cp_input_trace_t;
1902
1903 u8 *
1904 format_lisp_cp_input_trace (u8 * s, va_list * args)
1905 {
1906   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
1907   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
1908   CLIB_UNUSED(lisp_cp_input_trace_t * t) = va_arg (*args, lisp_cp_input_trace_t *);
1909
1910   s = format (s, "LISP-CP-INPUT: TODO");
1911   return s;
1912 }
1913
1914 ip_interface_address_t *
1915 ip_interface_get_first_interface_address (ip_lookup_main_t *lm, u32 sw_if_index,
1916                                           u8 loop)
1917 {
1918   vnet_main_t *vnm = vnet_get_main ();
1919   vnet_sw_interface_t * swif = vnet_get_sw_interface (vnm, sw_if_index);
1920   if (loop && swif->flags & VNET_SW_INTERFACE_FLAG_UNNUMBERED)
1921     sw_if_index = swif->unnumbered_sw_if_index;
1922   u32 ia =
1923       (vec_len((lm)->if_address_pool_index_by_sw_if_index) > (sw_if_index)) ?
1924           vec_elt((lm)->if_address_pool_index_by_sw_if_index, (sw_if_index)) :
1925           (u32) ~0;
1926   return pool_elt_at_index((lm)->if_address_pool, ia);
1927 }
1928
1929 void *
1930 ip_interface_get_first_ip_addres (ip_lookup_main_t *lm, u32 sw_if_index,
1931                                    u8 loop)
1932 {
1933   ip_interface_address_t * ia = ip_interface_get_first_interface_address (
1934       lm, sw_if_index, loop);
1935   return ip_interface_address_get_address (lm, ia);
1936 }
1937
1938 static void
1939 del_fwd_entry (lisp_cp_main_t * lcm, u32 src_map_index,
1940                u32 dst_map_index)
1941 {
1942   vnet_lisp_gpe_add_del_fwd_entry_args_t _a, * a = &_a;
1943   fwd_entry_t * fe = 0;
1944   uword * feip = 0;
1945   memset(a, 0, sizeof(*a));
1946
1947   feip = hash_get(lcm->fwd_entry_by_mapping_index, dst_map_index);
1948   if (!feip)
1949     return;
1950
1951   fe = pool_elt_at_index(lcm->fwd_entry_pool, feip[0]);
1952
1953   /* delete dp fwd entry */
1954   u32 sw_if_index;
1955   a->is_add = 0;
1956   a->dlocator = fe->dst_loc;
1957   a->slocator = fe->src_loc;
1958   a->vni = gid_address_vni(&a->deid);
1959   gid_address_copy(&a->deid, &fe->deid);
1960
1961   vnet_lisp_gpe_add_del_fwd_entry (a, &sw_if_index);
1962
1963   /* delete entry in fwd table */
1964   hash_unset(lcm->fwd_entry_by_mapping_index, dst_map_index);
1965   pool_put(lcm->fwd_entry_pool, fe);
1966 }
1967
1968 static void
1969 add_fwd_entry (lisp_cp_main_t* lcm, u32 src_map_index, u32 dst_map_index)
1970 {
1971   mapping_t * src_map, * dst_map;
1972   locator_set_t * dst_ls, * src_ls;
1973   u32 i, minp = ~0, sw_if_index;
1974   locator_t * dl = 0;
1975   uword * feip = 0, * tidp;
1976   fwd_entry_t* fe;
1977   vnet_lisp_gpe_add_del_fwd_entry_args_t _a, * a = &_a;
1978
1979   memset (a, 0, sizeof(*a));
1980
1981   /* remove entry if it already exists */
1982   feip = hash_get (lcm->fwd_entry_by_mapping_index, dst_map_index);
1983   if (feip)
1984     del_fwd_entry (lcm, src_map_index, dst_map_index);
1985
1986   src_map = pool_elt_at_index (lcm->mapping_pool, src_map_index);
1987   dst_map = pool_elt_at_index (lcm->mapping_pool, dst_map_index);
1988
1989   gid_address_copy (&a->deid, &dst_map->eid);
1990   a->vni = gid_address_vni(&a->deid);
1991
1992   tidp = hash_get(lcm->table_id_by_vni, a->vni);
1993   if (!tidp)
1994     {
1995       clib_warning("vni %d not associated to a vrf!", a->vni);
1996       return;
1997     }
1998   a->table_id = tidp[0];
1999
2000   /* XXX simple forwarding policy: first lowest (value) priority locator */
2001   dst_ls = pool_elt_at_index (lcm->locator_set_pool,
2002                               dst_map->locator_set_index);
2003   for (i = 0; i < vec_len (dst_ls->locator_indices); i++)
2004     {
2005       u32 li = vec_elt (dst_ls->locator_indices, i);
2006       locator_t * l = pool_elt_at_index (lcm->locator_pool, li);
2007       if (l->priority < minp && gid_address_type(&l->address)
2008             == GID_ADDR_IP_PREFIX)
2009         {
2010           minp = l->priority;
2011           dl = l;
2012         }
2013     }
2014   if (dl)
2015     {
2016       src_ls = pool_elt_at_index(lcm->locator_set_pool,
2017                                  src_map->locator_set_index);
2018       for (i = 0; i < vec_len(src_ls->locator_indices); i++)
2019         {
2020           u32 li = vec_elt (src_ls->locator_indices, i);
2021           locator_t * sl = pool_elt_at_index (lcm->locator_pool, li);
2022
2023           if (ip_addr_version(&gid_address_ip(&dl->address)) == IP4)
2024             {
2025               ip4_address_t * l4;
2026               l4 = ip_interface_get_first_ip_addres (&lcm->im4->lookup_main,
2027                                                      sl->sw_if_index,
2028                                                      1 /* unnumbered */);
2029               ip_addr_v4(&a->slocator) = *l4;
2030               ip_addr_version(&a->slocator) = IP4;
2031             }
2032           else
2033             {
2034               ip6_address_t * l6;
2035               l6 = ip_interface_get_first_ip_addres (&lcm->im6->lookup_main,
2036                                                      sl->sw_if_index,
2037                                                      1 /* unnumbered */);
2038               ip_addr_v6(&a->slocator) = *l6;
2039               ip_addr_version(&a->slocator) = IP6;
2040             }
2041         }
2042     }
2043
2044   /* insert data plane forwarding entry */
2045   a->is_add = 1;
2046   if (dl)
2047     a->dlocator = gid_address_ip(&dl->address);
2048   else
2049     {
2050       a->is_negative = 1;
2051       a->action = dst_map->action;
2052     }
2053
2054   /* TODO remove */
2055   u8 ipver = ip_prefix_version(&gid_address_ippref(&a->deid));
2056   a->decap_next_index = (ipver == IP4) ?
2057           LISP_GPE_INPUT_NEXT_IP4_INPUT : LISP_GPE_INPUT_NEXT_IP6_INPUT;
2058
2059   vnet_lisp_gpe_add_del_fwd_entry (a, &sw_if_index);
2060
2061   /* add tunnel to fwd entry table XXX check return value from DP insertion */
2062   pool_get (lcm->fwd_entry_pool, fe);
2063   fe->dst_loc = a->dlocator;
2064   fe->src_loc = a->slocator;
2065   gid_address_copy (&fe->deid, &a->deid);
2066   hash_set (lcm->fwd_entry_by_mapping_index, dst_map_index,
2067             fe - lcm->fwd_entry_pool);
2068 }
2069
2070 /* return 0 if the two locator sets are identical 1 otherwise */
2071 static u8
2072 compare_locators (lisp_cp_main_t *lcm, u32 * old_ls_indexes,
2073                   locator_t * new_locators)
2074 {
2075   u32 i, old_li;
2076   locator_t * old_loc, * new_loc;
2077
2078   if (vec_len (old_ls_indexes) != vec_len(new_locators))
2079     return 1;
2080
2081   for (i = 0; i < vec_len(new_locators); i++)
2082     {
2083       old_li = vec_elt(old_ls_indexes, i);
2084       old_loc = pool_elt_at_index(lcm->locator_pool, old_li);
2085
2086       new_loc = vec_elt_at_index(new_locators, i);
2087
2088       if (locator_cmp (old_loc, new_loc))
2089         return 1;
2090     }
2091   return 0;
2092 }
2093
2094 void
2095 process_map_reply (lisp_cp_main_t * lcm, vlib_buffer_t * b)
2096 {
2097   mapping_t * old_map;
2098   locator_t * loc;
2099   u32 len = 0, i, ls_index = 0;
2100   void * h;
2101   vnet_lisp_add_del_locator_set_args_t _ls_arg, * ls_arg = &_ls_arg;
2102   vnet_lisp_add_del_mapping_args_t _m_args, * m_args = &_m_args;
2103   pending_map_request_t * pmr;
2104   locator_t probed;
2105   map_reply_hdr_t * mrep_hdr;
2106   u64 nonce;
2107   u32 dst_map_index, mi;
2108   uword * pmr_index;
2109
2110   mrep_hdr = vlib_buffer_get_current (b);
2111
2112   /* Check pending requests table and nonce */
2113   nonce = MREP_NONCE(mrep_hdr);
2114   pmr_index = hash_get(lcm->pending_map_requests_by_nonce, nonce);
2115   if (!pmr_index)
2116     {
2117       clib_warning("No pending map-request entry with nonce %lu!", nonce);
2118       return;
2119     }
2120   pmr = pool_elt_at_index(lcm->pending_map_requests_pool, pmr_index[0]);
2121
2122   vlib_buffer_pull (b, sizeof(*mrep_hdr));
2123
2124   for (i = 0; i < MREP_REC_COUNT(mrep_hdr); i++)
2125     {
2126       memset (ls_arg, 0, sizeof(*ls_arg));
2127       memset (m_args, 0, sizeof(*m_args));
2128
2129       h = vlib_buffer_get_current (b);
2130       m_args->ttl = clib_net_to_host_u32 (MAP_REC_TTL(h));
2131       m_args->action = MAP_REC_ACTION(h);
2132       m_args->authoritative = MAP_REC_AUTH(h);
2133
2134       len = lisp_msg_parse_mapping_record (b, &m_args->deid, &ls_arg->locators,
2135                                            &probed);
2136       if (len == ~0)
2137         {
2138           clib_warning ("Failed to parse mapping record!");
2139           vec_foreach (loc, ls_arg->locators)
2140             {
2141               locator_free (loc);
2142             }
2143           vec_free(ls_arg->locators);
2144           return;
2145         }
2146
2147       mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &m_args->deid);
2148       old_map = mi != ~0 ? pool_elt_at_index(lcm->mapping_pool, mi) : 0;
2149
2150       /* if mapping already exists, decide if locators (and forwarding) should
2151        * be updated and be done */
2152       if (old_map != 0 && !gid_address_cmp (&old_map->eid, &m_args->deid))
2153         {
2154           locator_set_t * old_ls;
2155
2156           /* update mapping attributes */
2157           old_map->action = m_args->action;
2158           old_map->authoritative = m_args->authoritative;
2159           old_map->ttl = m_args->ttl;
2160
2161           old_ls = pool_elt_at_index(lcm->locator_set_pool,
2162                                      old_map->locator_set_index);
2163           /* if the two locators are not equal, update them and forwarding
2164            * otherwise there's nothing to be done */
2165           if (compare_locators (lcm, old_ls->locator_indices, ls_arg->locators))
2166             {
2167               /* set locator-set index to overwrite */
2168               ls_arg->is_add = 1;
2169               ls_arg->index = old_map->locator_set_index;
2170               vnet_lisp_add_del_locator_set (ls_arg, 0);
2171               add_fwd_entry (lcm, pmr->src_mapping_index, mi);
2172             }
2173         }
2174       /* new mapping */
2175       else
2176         {
2177           /* add locator-set */
2178           ls_arg->is_add = 1;
2179           ls_arg->index = ~0;
2180           vnet_lisp_add_del_locator_set (ls_arg, &ls_index);
2181
2182           /* add mapping */
2183           m_args->is_add = 1;
2184           m_args->locator_set_index = ls_index;
2185           vnet_lisp_add_del_mapping (m_args, &dst_map_index);
2186
2187           /* add forwarding tunnel */
2188           add_fwd_entry (lcm, pmr->src_mapping_index, dst_map_index);
2189         }
2190       vec_free(ls_arg->locators);
2191     }
2192
2193   /* remove pending map request entry */
2194   hash_unset(lcm->pending_map_requests_by_nonce, nonce);
2195   pool_put(lcm->pending_map_requests_pool, pmr);
2196 }
2197
2198 void
2199 process_map_request (vlib_main_t * vm, lisp_cp_main_t * lcm, vlib_buffer_t * b)
2200 {
2201   map_request_hdr_t * mreq_hdr;
2202   gid_address_t src, dst;
2203 //  u64 nonce;
2204   u32 i, len = 0;
2205   gid_address_t * itr_rlocs = 0, * rloc;
2206
2207   mreq_hdr = vlib_buffer_get_current (b);
2208   vlib_buffer_pull (b, sizeof(*mreq_hdr));
2209
2210 //  nonce = MREQ_NONCE(mreq_hdr);
2211
2212   if (!MREQ_SMR(mreq_hdr)) {
2213       clib_warning("Only SMR Map-Requests supported for now!");
2214       return;
2215   }
2216
2217   /* parse src eid */
2218   len = lisp_msg_parse_addr (b, &src);
2219   if (len == ~0)
2220     return;
2221
2222   /* for now we don't do anything with the itr's rlocs */
2223   len = lisp_msg_parse_itr_rlocs (b, &itr_rlocs, MREQ_ITR_RLOC_COUNT(mreq_hdr) + 1);
2224   if (len == ~0)
2225     return;
2226
2227   /* TODO: RLOCs are currently unused, so free them for now */
2228   vec_foreach (rloc, itr_rlocs)
2229     {
2230       gid_address_free (rloc);
2231     }
2232
2233   /* parse eid records and send SMR-invoked map-requests */
2234   for (i = 0; i < MREQ_REC_COUNT(mreq_hdr); i++)
2235     {
2236       memset(&dst, 0, sizeof(dst));
2237       len = lisp_msg_parse_eid_rec (b, &dst);
2238       if (len == ~0)
2239         {
2240           clib_warning("Can't parse map-request EID-record");
2241           return;
2242         }
2243       /* send SMR-invoked map-requests */
2244       send_encapsulated_map_request (vm, lcm, &dst, &src, /* invoked */ 1);
2245     }
2246 }
2247
2248 static uword
2249 lisp_cp_input (vlib_main_t * vm, vlib_node_runtime_t * node,
2250                vlib_frame_t * from_frame)
2251 {
2252   u32 n_left_from, * from, * to_next_drop;
2253   lisp_msg_type_e type;
2254   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
2255
2256   from = vlib_frame_vector_args (from_frame);
2257   n_left_from = from_frame->n_vectors;
2258
2259
2260   while (n_left_from > 0)
2261     {
2262       u32 n_left_to_next_drop;
2263
2264       vlib_get_next_frame (vm, node, LISP_CP_INPUT_NEXT_DROP,
2265                            to_next_drop, n_left_to_next_drop);
2266       while (n_left_from > 0 && n_left_to_next_drop > 0)
2267         {
2268           u32 bi0;
2269           vlib_buffer_t * b0;
2270
2271           bi0 = from[0];
2272           from += 1;
2273           n_left_from -= 1;
2274           to_next_drop[0] = bi0;
2275           to_next_drop += 1;
2276           n_left_to_next_drop -= 1;
2277
2278           b0 = vlib_get_buffer (vm, bi0);
2279
2280           type = lisp_msg_type(vlib_buffer_get_current (b0));
2281           switch (type)
2282             {
2283             case LISP_MAP_REPLY:
2284               process_map_reply (lcm, b0);
2285               break;
2286             case LISP_MAP_REQUEST:
2287               process_map_request(vm, lcm, b0);
2288               break;
2289             default:
2290               clib_warning("Unsupported LISP message type %d", type);
2291               break;
2292             }
2293
2294           b0->error = node->errors[LISP_CP_INPUT_ERROR_DROP];
2295
2296           if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED))
2297             {
2298
2299             }
2300         }
2301
2302       vlib_put_next_frame (vm, node, LISP_CP_INPUT_NEXT_DROP, n_left_to_next_drop);
2303     }
2304   return from_frame->n_vectors;
2305 }
2306
2307 VLIB_REGISTER_NODE (lisp_cp_input_node) = {
2308   .function = lisp_cp_input,
2309   .name = "lisp-cp-input",
2310   .vector_size = sizeof (u32),
2311   .format_trace = format_lisp_cp_input_trace,
2312   .type = VLIB_NODE_TYPE_INTERNAL,
2313
2314   .n_errors = LISP_CP_INPUT_N_ERROR,
2315   .error_strings = lisp_cp_input_error_strings,
2316
2317   .n_next_nodes = LISP_CP_INPUT_N_NEXT,
2318
2319   .next_nodes = {
2320       [LISP_CP_INPUT_NEXT_DROP] = "error-drop",
2321   },
2322 };
2323
2324 clib_error_t *
2325 lisp_cp_init (vlib_main_t *vm)
2326 {
2327   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
2328   clib_error_t * error = 0;
2329
2330   if ((error = vlib_call_init_function (vm, lisp_gpe_init)))
2331     return error;
2332
2333   lcm->im4 = &ip4_main;
2334   lcm->im6 = &ip6_main;
2335   lcm->vlib_main = vm;
2336   lcm->vnet_main = vnet_get_main();
2337
2338   gid_dictionary_init (&lcm->mapping_index_by_gid);
2339
2340   /* default vrf mapped to vni 0 */
2341   hash_set(lcm->table_id_by_vni, 0, 0);
2342
2343   udp_register_dst_port (vm, UDP_DST_PORT_lisp_cp,
2344                          lisp_cp_input_node.index, 1 /* is_ip4 */);
2345   udp_register_dst_port (vm, UDP_DST_PORT_lisp_cp6,
2346                          lisp_cp_input_node.index, 0 /* is_ip4 */);
2347
2348   return 0;
2349 }
2350
2351 VLIB_INIT_FUNCTION(lisp_cp_init);