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