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 /* Adds mapping to map-cache but does NOT program LISP forwarding */
  22 int
  23 vnet_lisp_add_del_mapping (vnet_lisp_add_del_mapping_args_t * a,
  24                            u32 * map_index_result)
  25 {
  26   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
  27   u32 mi, * map_indexp, map_index, i;
  28   mapping_t * m;
  29   u32 ** eid_indexes;
  30
  31   mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &a->deid);
  32   if (a->is_add)
  33     {
  34       /* TODO check if overwriting and take appropriate actions */
  35       if (mi != GID_LOOKUP_MISS) {
  36           clib_warning("eid %U found in the eid-table", format_ip_address,
  37                        &a->deid);
  38           return VNET_API_ERROR_VALUE_EXIST;
  39       }
  40
  41       pool_get(lcm->mapping_pool, m);
  42       m->eid = a->deid;
  43       m->locator_set_index = a->locator_set_index;
  44       m->ttl = a->ttl;
  45       m->local = a->local;
  46
  47       map_index = m - lcm->mapping_pool;
  48       gid_dictionary_add_del (&lcm->mapping_index_by_gid, &a->deid, map_index,
  49                               1);
  50
  51       if (pool_is_free_index(lcm->locator_set_pool, a->locator_set_index))
  52         {
  53           clib_warning("Locator set with index %d doesn't exist",
  54                        a->locator_set_index);
  55           return VNET_API_ERROR_INVALID_VALUE;
  56         }
  57
  58       /* add eid to list of eids supported by locator-set */
  59       vec_validate (lcm->locator_set_to_eids, a->locator_set_index);
  60       eid_indexes = vec_elt_at_index(lcm->locator_set_to_eids,
  61                                      a->locator_set_index);
  62       vec_add1(eid_indexes[0], map_index);
  63
  64       if (a->local)
  65         {
  66           /* mark as local */
  67           vec_add1(lcm->local_mappings_indexes, map_index);
  68
  69           /* XXX do something else? */
  70         }
  71       map_index_result[0] = map_index;
  72     }
  73   else
  74     {
  75       if (mi == GID_LOOKUP_MISS) {
  76           clib_warning("eid %U not found in the eid-table", format_ip_address,
  77                        &a->deid);
  78           return VNET_API_ERROR_INVALID_VALUE;
  79       }
  80
  81       /* clear locator-set to eids binding */
  82       eid_indexes = vec_elt_at_index(lcm->locator_set_to_eids,
  83                                      a->locator_set_index);
  84       for (i = 0; i < vec_len(eid_indexes[0]); i++)
  85         {
  86           map_indexp = vec_elt_at_index(eid_indexes[0], i);
  87           if (map_indexp[0] == mi)
  88               break;
  89         }
  90       vec_del1(eid_indexes[0], i);
  91
  92       /* remove local mark if needed */
  93       m = pool_elt_at_index(lcm->mapping_pool, mi);
  94       if (m->local)
  95         {
  96           u32 k, * lm_indexp;
  97           for (k = 0; k < vec_len(lcm->local_mappings_indexes); k++)
  98             {
  99               lm_indexp = vec_elt_at_index(lcm->local_mappings_indexes, k);
 100               if (lm_indexp[0] == mi)
 101                 break;
 102             }
 103           vec_del1(lcm->local_mappings_indexes, k);
 104         }
 105       else
 106         {
 107           /* remove tunnel ??? */
 108         }
 109
 110       /* remove mapping from dictionary */
 111       gid_dictionary_add_del (&lcm->mapping_index_by_gid, &a->deid, 0, 0);
 112       pool_put_index (lcm->mapping_pool, mi);
 113     }
 114
 115   return 0;
 116 }
 117
 118 static clib_error_t *
 119 lisp_add_del_local_eid_command_fn (vlib_main_t * vm, unformat_input_t * input,
 120                                    vlib_cli_command_t * cmd)
 121 {
 122   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 123   unformat_input_t _line_input, * line_input = &_line_input;
 124   u8 is_add = 1;
 125   gid_address_t eid;
 126   ip_prefix_t * prefp = &gid_address_ippref(&eid);
 127   gid_address_t * eids = 0;
 128   clib_error_t * error = 0;
 129   u8 * locator_set_name;
 130   u32 locator_set_index = 0, map_index = 0;
 131   uword * p;
 132   vnet_lisp_add_del_mapping_args_t _a, * a = &_a;
 133
 134   gid_address_type (&eid) = IP_PREFIX;
 135
 136   /* Get a line of input. */
 137   if (! unformat_user (input, unformat_line_input, line_input))
 138     return 0;
 139
 140   while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
 141     {
 142       if (unformat (line_input, "add"))
 143         is_add = 1;
 144       else if (unformat (line_input, "del"))
 145         is_add = 0;
 146       else if (unformat (line_input, "eid %U", unformat_ip_prefix, prefp))
 147         {
 148           vec_add1(eids, eid);
 149         }
 150       else if (unformat (line_input, "locator-set %_%v%_", &locator_set_name))
 151         {
 152           p = hash_get_mem(lcm->locator_set_index_by_name, locator_set_name);
 153           if (!p)
 154             {
 155               error = clib_error_return(0, "locator-set %s doesn't exist",
 156                                         locator_set_name);
 157               goto done;
 158             }
 159           locator_set_index = p[0];
 160         }
 161       else
 162         {
 163           error = unformat_parse_error(line_input);
 164           goto done;
 165         }
 166     }
 167
 168   /* XXX treat batch configuration */
 169   a->deid = eid;
 170   a->is_add = is_add;
 171   a->locator_set_index = locator_set_index;
 172   a->local = 1;
 173
 174   vnet_lisp_add_del_mapping (a, &map_index);
 175  done:
 176   vec_free(eids);
 177   return error;
 178 }
 179
 180 VLIB_CLI_COMMAND (lisp_add_del_local_eid_command) = {
 181     .path = "lisp eid-table",
 182     .short_help = "lisp eid-table add/del eid <eid> locator-set <locator-set>",
 183     .function = lisp_add_del_local_eid_command_fn,
 184 };
 185
 186 static clib_error_t *
 187 lisp_show_local_eid_table_command_fn (vlib_main_t * vm,
 188                                       unformat_input_t * input,
 189                                       vlib_cli_command_t * cmd)
 190 {
 191   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 192   mapping_t * mapit;
 193
 194   vlib_cli_output (vm, "%=20s%=16s", "EID", "Locator");
 195   pool_foreach (mapit, lcm->mapping_pool,
 196   ({
 197     u8 * msg = 0;
 198     locator_set_t * ls = pool_elt_at_index (lcm->locator_set_pool,
 199                                             mapit->locator_set_index);
 200     vlib_cli_output (vm, "%-16U%16v", format_gid_address, &mapit->eid,
 201                      ls->name);
 202     vec_free (msg);
 203   }));
 204
 205   return 0;
 206 }
 207
 208 VLIB_CLI_COMMAND (lisp_cp_show_local_eid_table_command) = {
 209     .path = "show lisp eid-table",
 210     .short_help = "Shows local EID table",
 211     .function = lisp_show_local_eid_table_command_fn,
 212 };
 213
 214 /* cleans locator to locator-set data and removes locators not part of
 215  * any locator-set */
 216 static void
 217 clean_locator_to_locator_set (lisp_cp_main_t * lcm, u32 lsi)
 218 {
 219   u32 i, j, *loc_indexp, *ls_indexp, **ls_indexes;
 220   locator_set_t * ls = pool_elt_at_index(lcm->locator_set_pool, lsi);
 221   for (i = 0; i < vec_len(ls->locator_indices); i++)
 222     {
 223       loc_indexp = vec_elt_at_index(ls->locator_indices, i);
 224       ls_indexes = vec_elt_at_index(lcm->locator_to_locator_sets,
 225                                     loc_indexp[0]);
 226       for (j = 0; j < vec_len(ls_indexes[0]); j++)
 227         {
 228           ls_indexp = vec_elt_at_index(ls_indexes[0], j);
 229           if (ls_indexp[0] == lsi)
 230             break;
 231         }
 232
 233       /* delete index for removed locator-set*/
 234       vec_del1(ls_indexes[0], j);
 235
 236       /* delete locator if it's part of no locator-set */
 237       if (vec_len (ls_indexes[0]) == 0)
 238         pool_put_index(lcm->locator_pool, loc_indexp[0]);
 239     }
 240 }
 241 int
 242 vnet_lisp_add_del_locator_set (vnet_lisp_add_del_locator_set_args_t * a,
 243                                u32 * ls_result)
 244 {
 245   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 246   locator_set_t * ls;
 247   locator_t * loc, * itloc;
 248   uword _p = (u32)~0, * p = &_p;
 249   u32 loc_index, ls_index, ** ls_indexes;
 250   u32 **eid_indexes;
 251
 252   if (a->is_add)
 253     {
 254       /* check if overwrite */
 255       if (a->local)
 256         p = hash_get_mem(lcm->locator_set_index_by_name, a->name);
 257       else
 258         *p = a->index;
 259
 260       /* overwrite */
 261       if (p && p[0] != (u32)~0)
 262         {
 263           ls = pool_elt_at_index(lcm->locator_set_pool, p[0]);
 264           if (!ls)
 265             {
 266               clib_warning("locator-set %d to be overwritten doesn't exist!",
 267                            p[0]);
 268               return -1;
 269             }
 270
 271           /* clean locator to locator-set vectors and remove locators if
 272            * they're not part of another locator-set */
 273           clean_locator_to_locator_set (lcm, p[0]);
 274
 275           /* remove locator indices from locator set */
 276           vec_free(ls->locator_indices);
 277
 278           ls_index = p[0];
 279
 280           if (ls_result)
 281             ls_result[0] = p[0];
 282         }
 283       /* new locator-set */
 284       else
 285         {
 286           pool_get(lcm->locator_set_pool, ls);
 287           ls_index = ls - lcm->locator_set_pool;
 288
 289           if (a->local)
 290             {
 291               ls->name = vec_dup(a->name);
 292
 293               if (!lcm->locator_set_index_by_name)
 294                 lcm->locator_set_index_by_name = hash_create_vec(
 295                     /* size */0, sizeof(ls->name[0]), sizeof(uword));
 296               hash_set_mem(lcm->locator_set_index_by_name, ls->name, ls_index);
 297
 298               /* mark as local locator-set */
 299               vec_add1(lcm->local_locator_set_indexes, ls_index);
 300             }
 301           ls->local = a->local;
 302           if (ls_result)
 303             ls_result[0] = ls_index;
 304         }
 305
 306       /* allocate locators */
 307       vec_foreach (itloc, a->locators)
 308         {
 309           pool_get(lcm->locator_pool, loc);
 310           loc[0] = itloc[0];
 311           loc_index = loc - lcm->locator_pool;
 312
 313           vec_add1(ls->locator_indices, loc_index);
 314
 315           vec_validate (lcm->locator_to_locator_sets, loc_index);
 316           ls_indexes = vec_elt_at_index(lcm->locator_to_locator_sets,
 317                                         loc_index);
 318           vec_add1(ls_indexes[0], ls_index);
 319         }
 320     }
 321   else
 322     {
 323       /* find locator-set */
 324       if (a->local)
 325         {
 326           p = hash_get_mem(lcm->locator_set_index_by_name, a->name);
 327           if (!p)
 328             {
 329               clib_warning("locator-set %v doesn't exists", a->name);
 330               return -1;
 331             }
 332         }
 333       else
 334         *p = a->index;
 335
 336       ls = pool_elt_at_index(lcm->locator_set_pool, p[0]);
 337       if (!ls)
 338         {
 339           clib_warning("locator-set with index %d doesn't exists", p[0]);
 340           return -1;
 341         }
 342 //      /* XXX what happens when a mapping is configured to use the loc-set ? */
 343 //      if (vec_len (vec_elt_at_index(lcm->locator_set_to_eids, p[0])) != 0)
 344 //        {
 345 //          clib_warning ("Can't delete a locator that supports a mapping!");
 346 //          return -1;
 347 //        }
 348
 349       if (vec_len(lcm->locator_set_to_eids) != 0)
 350       {
 351           eid_indexes = vec_elt_at_index(lcm->locator_set_to_eids, p[0]);
 352           if (vec_len(eid_indexes[0]) != 0)
 353           {
 354               clib_warning ("Can't delete a locator that supports a mapping!");
 355               return -1;
 356           }
 357       }
 358
 359       /* clean locator to locator-sets data */
 360       clean_locator_to_locator_set (lcm, p[0]);
 361
 362       if (ls->local)
 363         {
 364           u32 it, lsi;
 365
 366           vec_foreach_index(it, lcm->local_locator_set_indexes)
 367           {
 368             lsi = vec_elt(lcm->local_locator_set_indexes, it);
 369             if (lsi == p[0])
 370               {
 371                 vec_del1(lcm->local_locator_set_indexes, it);
 372                 break;
 373               }
 374           }
 375           hash_unset_mem(lcm->locator_set_index_by_name, ls->name);
 376           vec_free(ls->name);
 377         }
 378       pool_put(lcm->locator_set_pool, ls);
 379     }
 380   return 0;
 381 }
 382
 383 static inline
 384 uword *vnet_lisp_get_locator(vnet_lisp_add_del_locator_set_args_t * a,
 385                              uword *p)
 386 {
 387   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 388
 389   ASSERT(a != NULL);
 390   ASSERT(p != NULL);
 391
 392   /* find locator-set */
 393   if (a->local)
 394   {
 395       p = hash_get_mem(lcm->locator_set_index_by_name, a->name);
 396   }
 397   else
 398   {
 399       *p = a->index;
 400   }
 401
 402   return p;
 403 }
 404
 405 int
 406 vnet_lisp_add_del_locator_set_name (vnet_lisp_add_del_locator_set_args_t * a,
 407                                     u32 * ls_result)
 408 {
 409   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 410   locator_set_t * ls;
 411   uword _p = (u32)~0, * p = &_p;
 412   u32 ls_index = ~0;
 413   u32 **eid_indexes = NULL;
 414
 415   ASSERT(a != NULL);
 416   ASSERT(ls_result != NULL);
 417
 418   p = vnet_lisp_get_locator(a, p);
 419
 420   if (a->is_add)
 421     {
 422       /* overwrite */
 423       if (p && p[0] != (u32)~0)
 424         {
 425           ls = pool_elt_at_index(lcm->locator_set_pool, p[0]);
 426           if (!ls)
 427             {
 428               clib_warning("locator-set %d to be overwritten doesn't exist!",
 429                            p[0]);
 430               return VNET_API_ERROR_UNSPECIFIED;
 431             }
 432
 433           /* clean locator to locator-set vectors and remove locators if
 434            * they're not part of another locator-set */
 435           clean_locator_to_locator_set (lcm, p[0]);
 436
 437           /* remove locator indices from locator set */
 438           vec_free(ls->locator_indices);
 439
 440           ls_index = p[0];
 441
 442           if (ls_result)
 443             ls_result[0] = p[0];
 444         }
 445       /* new locator-set */
 446       else
 447         {
 448           pool_get(lcm->locator_set_pool, ls);
 449           ls_index = ls - lcm->locator_set_pool;
 450
 451           if (a->local)
 452             {
 453               ls->name = vec_dup(a->name);
 454
 455               if (!lcm->locator_set_index_by_name)
 456                 lcm->locator_set_index_by_name = hash_create_vec(
 457                     /* size */0, sizeof(ls->name[0]), sizeof(uword));
 458               hash_set_mem(lcm->locator_set_index_by_name, ls->name, ls_index);
 459
 460               /* mark as local locator-set */
 461               vec_add1(lcm->local_locator_set_indexes, ls_index);
 462             }
 463           ls->local = a->local;
 464           ls->locator_indices = NULL;
 465           if (ls_result)
 466             ls_result[0] = ls_index;
 467         }
 468     }
 469   else
 470     {
 471        if (!p)
 472        {
 473            clib_warning("locator-set %v doesn't exists", a->name);
 474            return VNET_API_ERROR_INVALID_ARGUMENT;
 475        }
 476
 477        ls = pool_elt_at_index(lcm->locator_set_pool, p[0]);
 478        if (!ls)
 479        {
 480            clib_warning("locator-set with index %d doesn't exists", p[0]);
 481            return VNET_API_ERROR_INVALID_ARGUMENT;
 482        }
 483
 484       if (vec_len(lcm->locator_set_to_eids) != 0)
 485       {
 486           eid_indexes = vec_elt_at_index(lcm->locator_set_to_eids, p[0]);
 487           if (vec_len(eid_indexes[0]) != 0)
 488           {
 489               clib_warning ("Can't delete a locator that supports a mapping!");
 490               return -1;
 491           }
 492       }
 493
 494       /* clean locator to locator-sets data */
 495       clean_locator_to_locator_set (lcm, p[0]);
 496
 497       if (ls->local)
 498         {
 499           u32 it, lsi;
 500
 501           vec_foreach_index(it, lcm->local_locator_set_indexes)
 502             {
 503               lsi = vec_elt(lcm->local_locator_set_indexes, it);
 504               if (lsi == p[0])
 505                 {
 506                   vec_del1(lcm->local_locator_set_indexes, it);
 507                   break;
 508                 }
 509             }
 510           hash_unset_mem(lcm->locator_set_index_by_name, ls->name);
 511           vec_free(ls->name);
 512         }
 513       pool_put(lcm->locator_set_pool, ls);
 514     }
 515   return 0;
 516 }
 517
 518 int
 519 vnet_lisp_add_del_locator (vnet_lisp_add_del_locator_set_args_t *a,
 520                            u32 *ls_result)
 521 {
 522   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 523   locator_set_t *ls = NULL;
 524   locator_t *loc = NULL, *itloc = NULL;
 525   uword _p = (u32)~0, * p = &_p;
 526   u32 loc_index = ~0, ls_index = ~0, *locit = NULL, **ls_indexes = NULL;
 527   u32 i = ~0;
 528
 529   ASSERT(a != NULL);
 530   ASSERT(ls_result != NULL);
 531
 532   p = vnet_lisp_get_locator(a, p);
 533   if (!p) {
 534       clib_warning("locator-set %v doesn't exists", a->name);
 535       return VNET_API_ERROR_INVALID_ARGUMENT;
 536   }
 537
 538   ls_index = p[0];
 539
 540   if (a->is_add)
 541     {
 542         ls = pool_elt_at_index(lcm->locator_set_pool, p[0]);
 543         if (!ls)
 544         {
 545             clib_warning("locator-set %d to be overwritten doesn't exist!",
 546                          p[0]);
 547             return VNET_API_ERROR_INVALID_ARGUMENT;
 548         }
 549
 550         if (ls_result)
 551             ls_result[0] = p[0];
 552
 553       /* allocate locators */
 554       itloc = a->locators;
 555       pool_get(lcm->locator_pool, loc);
 556       loc[0] = itloc[0];
 557       loc_index = loc - lcm->locator_pool;
 558
 559       vec_add1(ls->locator_indices, loc_index);
 560
 561       vec_validate (lcm->locator_to_locator_sets, loc_index);
 562       ls_indexes = vec_elt_at_index(lcm->locator_to_locator_sets,
 563                                     loc_index);
 564       vec_add1(ls_indexes[0], ls_index);
 565     }
 566   else
 567     {
 568       ls = pool_elt_at_index(lcm->locator_set_pool, p[0]);
 569       if (!ls)
 570         {
 571           clib_warning("locator-set with index %d doesn't exists", p[0]);
 572           return VNET_API_ERROR_INVALID_ARGUMENT;
 573         }
 574
 575       if (ls->local)
 576       {
 577           itloc = a->locators;
 578           i = 0;
 579           vec_foreach (locit, ls->locator_indices)
 580           {
 581               loc = pool_elt_at_index(lcm->locator_pool, locit[0]);
 582               if (loc->local && loc->sw_if_index == itloc->sw_if_index)
 583               {
 584                   ls_indexes = vec_elt_at_index(lcm->locator_to_locator_sets,
 585                                                 locit[0]);
 586                   pool_put_index(lcm->locator_pool, locit[0]);
 587                   vec_del1(ls->locator_indices, i);
 588                   vec_del1(ls_indexes[0], ls_index);
 589               }
 590               i++;
 591           }
 592       }
 593     }
 594   return 0;
 595 }
 596
 597 static clib_error_t *
 598 lisp_add_del_locator_set_command_fn (vlib_main_t * vm, unformat_input_t * input,
 599                                      vlib_cli_command_t * cmd)
 600 {
 601   lisp_gpe_main_t * lgm = &lisp_gpe_main;
 602   vnet_main_t * vnm = lgm->vnet_main;
 603   unformat_input_t _line_input, * line_input = &_line_input;
 604   u8 is_add = 1;
 605   clib_error_t * error = 0;
 606   u8 * locator_set_name = 0;
 607   locator_t locator, * locators = 0;
 608   vnet_lisp_add_del_locator_set_args_t _a, * a = &_a;
 609   u32 ls_index = 0;
 610
 611   memset(&locator, 0, sizeof(locator));
 612   memset(a, 0, sizeof(a[0]));
 613
 614   /* Get a line of input. */
 615   if (! unformat_user (input, unformat_line_input, line_input))
 616     return 0;
 617
 618   while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
 619     {
 620       if (unformat (line_input, "add %_%v%_", &locator_set_name))
 621         is_add = 1;
 622       else if (unformat (line_input, "del %_%v%_", &locator_set_name))
 623         is_add = 0;
 624       else if (unformat (line_input, "iface %U p %d w %d",
 625                          unformat_vnet_sw_interface, vnm, &locator.sw_if_index,
 626                          &locator.priority, &locator.weight))
 627         {
 628           locator.local = 1;
 629           vec_add1(locators, locator);
 630         }
 631       else
 632         {
 633           error = unformat_parse_error(line_input);
 634           goto done;
 635         }
 636     }
 637
 638   a->name = locator_set_name;
 639   a->locators = locators;
 640   a->is_add = is_add;
 641   a->local = 1;
 642
 643   vnet_lisp_add_del_locator_set(a, &ls_index);
 644
 645  done:
 646   vec_free(locators);
 647   vec_free(locator_set_name);
 648   return error;
 649 }
 650
 651 VLIB_CLI_COMMAND (lisp_cp_add_del_locator_set_command) = {
 652     .path = "lisp locator-set",
 653     .short_help = "lisp locator-set add/del <name> <iface-name> <priority> <weight>",
 654     .function = lisp_add_del_locator_set_command_fn,
 655 };
 656
 657 static clib_error_t *
 658 lisp_cp_show_locator_sets_command_fn (vlib_main_t * vm,
 659                                       unformat_input_t * input,
 660                                       vlib_cli_command_t * cmd)
 661 {
 662   locator_set_t * lsit;
 663   locator_t * loc;
 664   u32 * locit;
 665   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 666
 667   vlib_cli_output (vm, "%=20s%=16s%=16s%=16s", "Locator-set", "Locator",
 668                    "Priority", "Weight");
 669   pool_foreach (lsit, lcm->locator_set_pool,
 670   ({
 671     u8 * msg = 0;
 672     msg = format (msg, "%-16v", lsit->name);
 673     vec_foreach (locit, lsit->locator_indices)
 674       {
 675         loc = pool_elt_at_index (lcm->locator_pool, locit[0]);
 676         if (loc->local)
 677           msg = format (msg, "%16d%16d%16d", loc->sw_if_index, loc->priority,
 678                         loc->weight);
 679         else
 680           msg = format (msg, "%16U%16d%16d", format_gid_address, &loc->address,
 681                         loc->priority, loc->weight);
 682       }
 683     vlib_cli_output (vm, "%v", msg);
 684     vec_free (msg);
 685   }));
 686   return 0;
 687 }
 688
 689 VLIB_CLI_COMMAND (lisp_cp_show_locator_sets_command) = {
 690     .path = "show lisp locator-set",
 691     .short_help = "Shows locator-sets",
 692     .function = lisp_cp_show_locator_sets_command_fn,
 693 };
 694
 695 int
 696 vnet_lisp_add_del_map_resolver (vnet_lisp_add_del_map_resolver_args_t * a)
 697 {
 698   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 699   ip_address_t * addr;
 700   u32 i;
 701
 702   if (a->is_add)
 703     {
 704       vec_foreach(addr, lcm->map_resolvers)
 705         {
 706           if (!ip_address_cmp (addr, &a->address))
 707             {
 708               clib_warning("map-resolver %U already exists!", format_ip_address,
 709                            &a->address);
 710               return -1;
 711             }
 712         }
 713       vec_add1(lcm->map_resolvers, a->address);
 714     }
 715   else
 716     {
 717       for (i = 0; i < vec_len(lcm->map_resolvers); i++)
 718         {
 719           addr = vec_elt_at_index(lcm->map_resolvers, i);
 720           if (!ip_address_cmp (addr, &a->address))
 721             {
 722               vec_delete(lcm->map_resolvers, 1, i);
 723               break;
 724             }
 725         }
 726     }
 727   return 0;
 728 }
 729
 730 static clib_error_t *
 731 lisp_add_del_map_resolver_command_fn (vlib_main_t * vm,
 732                                          unformat_input_t * input,
 733                                          vlib_cli_command_t * cmd)
 734 {
 735   unformat_input_t _line_input, * line_input = &_line_input;
 736   u8 is_add = 1;
 737   ip_address_t ip_addr;
 738   clib_error_t * error = 0;
 739   vnet_lisp_add_del_map_resolver_args_t _a, * a = &_a;
 740
 741   /* Get a line of input. */
 742   if (! unformat_user (input, unformat_line_input, line_input))
 743     return 0;
 744
 745   while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
 746     {
 747       if (unformat (line_input, "add"))
 748         is_add = 1;
 749       else if (unformat (line_input, "del"))
 750         is_add = 0;
 751       else if (unformat (line_input, "%U", unformat_ip_address, &ip_addr))
 752         ;
 753       else
 754         {
 755           error = unformat_parse_error(line_input);
 756           goto done;
 757         }
 758     }
 759   a->is_add = is_add;
 760   a->address = ip_addr;
 761   vnet_lisp_add_del_map_resolver (a);
 762
 763  done:
 764   return error;
 765 }
 766
 767 VLIB_CLI_COMMAND (lisp_add_del_map_resolver_command) = {
 768     .path = "lisp map-resolver",
 769     .short_help = "lisp map-resolver add/del <ip_address>",
 770     .function = lisp_add_del_map_resolver_command_fn,
 771 };
 772
 773 /* Statistics (not really errors) */
 774 #define foreach_lisp_cp_lookup_error           \
 775 _(DROP, "drop")                                \
 776 _(MAP_REQUESTS_SENT, "map-request sent")
 777
 778 static char * lisp_cp_lookup_error_strings[] = {
 779 #define _(sym,string) string,
 780   foreach_lisp_cp_lookup_error
 781 #undef _
 782 };
 783
 784 typedef enum
 785 {
 786 #define _(sym,str) LISP_CP_LOOKUP_ERROR_##sym,
 787     foreach_lisp_cp_lookup_error
 788 #undef _
 789     LISP_CP_LOOKUP_N_ERROR,
 790 } lisp_cp_lookup_error_t;
 791
 792 typedef enum
 793 {
 794   LISP_CP_LOOKUP_NEXT_DROP,
 795   LISP_CP_LOOKUP_NEXT_IP4_LOOKUP,
 796   LISP_CP_LOOKUP_NEXT_IP6_LOOKUP,
 797   LISP_CP_LOOKUP_N_NEXT,
 798 } lisp_cp_lookup_next_t;
 799
 800 typedef struct
 801 {
 802   gid_address_t dst_eid;
 803   ip4_address_t map_resolver_ip;
 804 } lisp_cp_lookup_trace_t;
 805
 806 u8 *
 807 format_lisp_cp_lookup_trace (u8 * s, va_list * args)
 808 {
 809   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
 810   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
 811   lisp_cp_lookup_trace_t * t = va_arg (*args, lisp_cp_lookup_trace_t *);
 812
 813   s = format (s, "LISP-CP-LOOKUP: map-resolver: %U destination eid %U",
 814               format_ip4_address, &t->map_resolver_ip, format_gid_address,
 815               &t->dst_eid);
 816   return s;
 817 }
 818
 819 static u32
 820 ip_fib_lookup_with_table (lisp_cp_main_t * lcm, u32 fib_index,
 821                           ip_address_t * dst)
 822 {
 823   if (ip_addr_version (dst) == IP4)
 824       return ip4_fib_lookup_with_table (lcm->im4, fib_index, &ip_addr_v4(dst),
 825                                         0);
 826   else
 827       return ip6_fib_lookup_with_table (lcm->im6, fib_index, &ip_addr_v6(dst));
 828 }
 829
 830 void
 831 get_local_iface_ip_for_dst (lisp_cp_main_t *lcm, ip_address_t * dst,
 832                             ip_address_t * sloc)
 833 {
 834   u32 adj_index;
 835   ip_adjacency_t * adj;
 836   ip_interface_address_t * ia = 0;
 837   ip_lookup_main_t * lm = &lcm->im4->lookup_main;
 838   ip4_address_t * l4 = 0;
 839   ip6_address_t * l6 = 0;
 840
 841   adj_index = ip_fib_lookup_with_table (lcm, 0, dst);
 842   adj = ip_get_adjacency (lm, adj_index);
 843
 844   if (adj->lookup_next_index == IP_LOOKUP_NEXT_ARP)
 845     {
 846       ia = pool_elt_at_index(lm->if_address_pool, adj->if_address_index);
 847       if (ip_addr_version(dst) == IP4)
 848         {
 849           l4 = ip_interface_address_get_address (lm, ia);
 850         }
 851       else
 852         {
 853           l6 = ip_interface_address_get_address (lm, ia);
 854         }
 855     }
 856   else if (adj->lookup_next_index == IP_LOOKUP_NEXT_REWRITE)
 857     {
 858       /* find sw_if_index in rewrite header */
 859       u32 sw_if_index = adj->rewrite_header.sw_if_index;
 860
 861       /* find suitable address */
 862       if (ip_addr_version(dst) == IP4)
 863         {
 864           /* find the first ip address */
 865           foreach_ip_interface_address (&lcm->im4->lookup_main, ia,
 866                                         sw_if_index, 1 /* unnumbered */,
 867           ({
 868             l4 = ip_interface_address_get_address (&lcm->im4->lookup_main, ia);
 869             break;
 870           }));
 871         }
 872       else
 873         {
 874           /* find the first ip address */
 875           foreach_ip_interface_address (&lcm->im6->lookup_main, ia,
 876                                         sw_if_index, 1 /* unnumbered */,
 877           ({
 878             l6 = ip_interface_address_get_address (&lcm->im6->lookup_main, ia);
 879             break;
 880           }));
 881         }
 882     }
 883   else
 884     {
 885       clib_warning("Can't find local local interface ip for dst %U",
 886                    format_ip_address, dst);
 887       return;
 888     }
 889
 890   if (l4)
 891     {
 892       ip_addr_v4(sloc).as_u32 = l4->as_u32;
 893       ip_addr_version(sloc) = IP4;
 894     }
 895   else if (l6)
 896     {
 897       memcpy (&ip_addr_v6(sloc), l6, sizeof(*l6));
 898       ip_addr_version(sloc) = IP6;
 899     }
 900   else
 901     {
 902       clib_warning("Can't find local interface addr for dst %U",
 903                    format_ip_address, dst);
 904     }
 905 }
 906
 907
 908 static ip_address_t *
 909 build_itr_rloc_list (lisp_cp_main_t * lcm, locator_set_t * loc_set)
 910 {
 911   ip4_address_t * l4;
 912   ip6_address_t * l6;
 913   u32 i;
 914   locator_t * loc;
 915   u32 * loc_indexp;
 916   ip_interface_address_t * ia = 0;
 917   ip_address_t * rlocs = 0;
 918   ip_address_t _rloc, * rloc = &_rloc;
 919
 920   for (i = 0; i < vec_len(loc_set->locator_indices); i++)
 921     {
 922       loc_indexp = vec_elt_at_index(loc_set->locator_indices, i);
 923       loc = pool_elt_at_index (lcm->locator_pool, loc_indexp[0]);
 924
 925       ip_addr_version(rloc) = IP4;
 926       /* Add ipv4 locators first TODO sort them */
 927       foreach_ip_interface_address (&lcm->im4->lookup_main, ia,
 928                                     loc->sw_if_index, 1 /* unnumbered */,
 929       ({
 930         l4 = ip_interface_address_get_address (&lcm->im4->lookup_main, ia);
 931   ip_addr_v4(rloc) = l4[0];
 932   vec_add1(rlocs, rloc[0]);
 933       }));
 934
 935       ip_addr_version(rloc) = IP6;
 936       /* Add ipv6 locators */
 937       foreach_ip_interface_address (&lcm->im6->lookup_main, ia,
 938                                     loc->sw_if_index, 1 /* unnumbered */,
 939       ({
 940   l6 = ip_interface_address_get_address (&lcm->im6->lookup_main, ia);
 941   ip_addr_v6(rloc) = l6[0];
 942   vec_add1(rlocs, rloc[0]);
 943       }));
 944     }
 945   return rlocs;
 946 }
 947
 948 static vlib_buffer_t *
 949 build_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
 950                                 gid_address_t * seid, gid_address_t * deid,
 951                                 locator_set_t * loc_set, u8 is_smr_invoked,
 952                                 u64 *nonce_res, u32 * bi_res)
 953 {
 954   vlib_buffer_t * b;
 955   u32 bi;
 956   ip_address_t * mr_ip, sloc;
 957   ip_address_t * rlocs = 0;
 958
 959   if (vlib_buffer_alloc (vm, &bi, 1) != 1)
 960     {
 961       clib_warning ("Can't allocate buffer for Map-Request!");
 962       return 0;
 963     }
 964
 965   b = vlib_get_buffer (vm, bi);
 966
 967   /* leave some space for the encap headers */
 968   vlib_buffer_make_headroom (b, MAX_LISP_MSG_ENCAP_LEN);
 969
 970   /* get rlocs */
 971   rlocs = build_itr_rloc_list (lcm, loc_set);
 972
 973   /* put lisp msg */
 974   lisp_msg_put_mreq (lcm, b, seid, deid, rlocs, is_smr_invoked, nonce_res);
 975
 976   /* push ecm: udp-ip-lisp */
 977   lisp_msg_push_ecm (vm, b, LISP_CONTROL_PORT, LISP_CONTROL_PORT, seid, deid);
 978
 979   /* get map-resolver ip XXX use first*/
 980   mr_ip = vec_elt_at_index(lcm->map_resolvers, 0);
 981
 982   /* get local iface ip to use in map-request XXX fib 0 for now*/
 983   get_local_iface_ip_for_dst (lcm, mr_ip, &sloc);
 984
 985   /* push outer ip header */
 986   pkt_push_udp_and_ip (vm, b, LISP_CONTROL_PORT, LISP_CONTROL_PORT, &sloc,
 987                        mr_ip);
 988
 989   bi_res[0] = bi;
 990
 991   if (rlocs)
 992     vec_free(rlocs);
 993   return b;
 994 }
 995
 996 static void
 997 send_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
 998                                gid_address_t * seid, gid_address_t * deid,
 999                                u8 is_smr_invoked)
1000 {
1001   u32 next_index, bi = 0, * to_next, map_index;
1002   vlib_buffer_t * b;
1003   vlib_frame_t * f;
1004   u64 nonce = 0;
1005   locator_set_t * loc_set;
1006   mapping_t * map;
1007   pending_map_request_t * pmr;
1008
1009   /* get locator-set for seid */
1010   map_index = gid_dictionary_lookup (&lcm->mapping_index_by_gid, seid);
1011   if (map_index == ~0)
1012     {
1013       clib_warning("No local mapping found in eid-table for %U!",
1014                    format_gid_address, seid);
1015       return;
1016     }
1017
1018   map = pool_elt_at_index (lcm->mapping_pool, map_index);
1019
1020   if (!map->local)
1021     {
1022       clib_warning("Mapping found for src eid %U is not marked as local!",
1023                    format_gid_address, seid);
1024       return;
1025     }
1026   loc_set = pool_elt_at_index (lcm->locator_set_pool, map->locator_set_index);
1027
1028   /* build the encapsulated map request */
1029   b = build_encapsulated_map_request (vm, lcm, seid, deid, loc_set,
1030                                       is_smr_invoked, &nonce, &bi);
1031
1032   if (!b)
1033     return;
1034
1035   vnet_buffer(b)->sw_if_index[VLIB_TX] = ~0;
1036   next_index = (ip_prefix_version(&gid_address_ippref(seid)) == IP4) ?
1037       ip4_lookup_node.index : ip6_lookup_node.index;
1038
1039   f = vlib_get_frame_to_node (vm, next_index);
1040
1041   /* Enqueue the packet */
1042   to_next = vlib_frame_vector_args (f);
1043   to_next[0] = bi;
1044   f->n_vectors = 1;
1045   vlib_put_frame_to_node (vm, next_index, f);
1046
1047   /* add map-request to pending requests table */
1048   pool_get(lcm->pending_map_requests_pool, pmr);
1049   gid_address_copy (&pmr->src, seid);
1050   gid_address_copy (&pmr->dst, deid);
1051   pmr->src_mapping_index = map_index;
1052   hash_set(lcm->pending_map_requests_by_nonce, nonce,
1053            pmr - lcm->pending_map_requests_pool);
1054 }
1055
1056 static void
1057 get_src_and_dst (void *hdr, ip_address_t * src, ip_address_t *dst)
1058 {
1059   ip4_header_t * ip4 = hdr;
1060   ip6_header_t * ip6;
1061
1062   if ((ip4->ip_version_and_header_length & 0xF0) == 0x40)
1063     {
1064       ip_addr_v4(src).as_u32 = ip4->src_address.as_u32;
1065       ip_addr_version(src) = IP4;
1066       ip_addr_v4(dst).as_u32 = ip4->dst_address.as_u32;
1067       ip_addr_version(dst) = IP4;
1068     }
1069   else
1070     {
1071       ip6 = hdr;
1072       memcpy (&ip_addr_v6(src), &ip6->src_address, sizeof(ip6->src_address));
1073       ip_addr_version(src) = IP6;
1074       memcpy (&ip_addr_v6(dst), &ip6->dst_address, sizeof(ip6->dst_address));
1075       ip_addr_version(dst) = IP6;
1076     }
1077 }
1078
1079 static uword
1080 lisp_cp_lookup (vlib_main_t * vm, vlib_node_runtime_t * node,
1081               vlib_frame_t * from_frame)
1082 {
1083   u32 * from, * to_next_drop;
1084   lisp_cp_main_t *lcm = vnet_lisp_cp_get_main();
1085   u32 pkts_mapped = 0;
1086   uword n_left_from, n_left_to_next_drop;
1087
1088   from = vlib_frame_vector_args (from_frame);
1089   n_left_from = from_frame->n_vectors;
1090
1091   while (n_left_from > 0)
1092     {
1093       vlib_get_next_frame (vm, node, LISP_CP_LOOKUP_NEXT_DROP,
1094                            to_next_drop, n_left_to_next_drop);
1095
1096       while (n_left_from > 0 && n_left_to_next_drop > 0)
1097         {
1098           u32 pi0;
1099           vlib_buffer_t * p0;
1100           ip4_header_t * ip0;
1101           gid_address_t src, dst;
1102           ip_prefix_t * spref, * dpref;
1103
1104           gid_address_type (&src) = IP_PREFIX;
1105           spref = &gid_address_ippref(&src);
1106           gid_address_type (&dst) = IP_PREFIX;
1107           dpref = &gid_address_ippref(&dst);
1108
1109           pi0 = from[0];
1110           from += 1;
1111           n_left_from -= 1;
1112           to_next_drop[0] = pi0;
1113           to_next_drop += 1;
1114           n_left_to_next_drop -= 1;
1115
1116           p0 = vlib_get_buffer (vm, pi0);
1117           p0->error = node->errors[LISP_CP_LOOKUP_ERROR_DROP];
1118
1119           /* src/dst eid pair */
1120           ip0 = vlib_buffer_get_current (p0);
1121           get_src_and_dst (ip0, &ip_prefix_addr(spref), &ip_prefix_addr(dpref));
1122           ip_prefix_len(spref) = ip_address_max_len (ip_prefix_version(spref));
1123           ip_prefix_len(dpref) = ip_address_max_len (ip_prefix_version(dpref));
1124
1125           /* send map-request */
1126           send_encapsulated_map_request (vm, lcm, &src, &dst, 0);
1127
1128           pkts_mapped++;
1129
1130           if (PREDICT_FALSE(p0->flags & VLIB_BUFFER_IS_TRACED))
1131             {
1132               lisp_cp_lookup_trace_t *tr = vlib_add_trace (vm, node, p0,
1133                                                           sizeof(*tr));
1134               gid_address_copy (&tr->dst_eid, &dst);
1135               memcpy (&tr->map_resolver_ip,
1136                       vec_elt_at_index(lcm->map_resolvers, 0),
1137                       sizeof(ip_address_t));
1138             }
1139         }
1140
1141       vlib_put_next_frame (vm, node, LISP_CP_LOOKUP_NEXT_DROP, n_left_to_next_drop);
1142     }
1143   vlib_node_increment_counter (vm, node->node_index,
1144                                LISP_CP_LOOKUP_ERROR_MAP_REQUESTS_SENT,
1145                                pkts_mapped);
1146   return from_frame->n_vectors;
1147 }
1148
1149 VLIB_REGISTER_NODE (lisp_cp_lookup_node) = {
1150   .function = lisp_cp_lookup,
1151   .name = "lisp-cp-lookup",
1152   .vector_size = sizeof (u32),
1153   .format_trace = format_lisp_cp_lookup_trace,
1154   .type = VLIB_NODE_TYPE_INTERNAL,
1155
1156   .n_errors = LISP_CP_LOOKUP_N_ERROR,
1157   .error_strings = lisp_cp_lookup_error_strings,
1158
1159   .n_next_nodes = LISP_CP_LOOKUP_N_NEXT,
1160
1161   .next_nodes = {
1162       [LISP_CP_LOOKUP_NEXT_DROP] = "error-drop",
1163       [LISP_CP_LOOKUP_NEXT_IP4_LOOKUP] = "ip4-lookup",
1164       [LISP_CP_LOOKUP_NEXT_IP6_LOOKUP] = "ip6-lookup",
1165   },
1166 };
1167
1168 /* lisp_cp_input statistics */
1169 #define foreach_lisp_cp_input_error                   \
1170 _(DROP, "drop")                                        \
1171 _(MAP_REPLIES_RECEIVED, "map-replies received")
1172
1173 static char * lisp_cp_input_error_strings[] = {
1174 #define _(sym,string) string,
1175   foreach_lisp_cp_input_error
1176 #undef _
1177 };
1178
1179 typedef enum
1180 {
1181 #define _(sym,str) LISP_CP_INPUT_ERROR_##sym,
1182     foreach_lisp_cp_input_error
1183 #undef _
1184     LISP_CP_INPUT_N_ERROR,
1185 } lisp_cp_input_error_t;
1186
1187 typedef enum
1188 {
1189   LISP_CP_INPUT_NEXT_DROP,
1190   LISP_CP_INPUT_N_NEXT,
1191 } lisp_cp_input_next_t;
1192
1193 typedef struct
1194 {
1195   gid_address_t dst_eid;
1196   ip4_address_t map_resolver_ip;
1197 } lisp_cp_input_trace_t;
1198
1199 u8 *
1200 format_lisp_cp_input_trace (u8 * s, va_list * args)
1201 {
1202   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
1203   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
1204   CLIB_UNUSED(lisp_cp_input_trace_t * t) = va_arg (*args, lisp_cp_input_trace_t *);
1205
1206   s = format (s, "LISP-CP-INPUT: TODO");
1207   return s;
1208 }
1209
1210 ip_interface_address_t *
1211 ip_interface_get_first_interface_address (ip_lookup_main_t *lm, u32 sw_if_index,
1212                                           u8 loop)
1213 {
1214   vnet_main_t *vnm = vnet_get_main ();
1215   vnet_sw_interface_t * swif = vnet_get_sw_interface (vnm, sw_if_index);
1216   if (loop && swif->flags & VNET_SW_INTERFACE_FLAG_UNNUMBERED)
1217     sw_if_index = swif->unnumbered_sw_if_index;
1218   u32 ia =
1219       (vec_len((lm)->if_address_pool_index_by_sw_if_index) > (sw_if_index)) ?
1220           vec_elt((lm)->if_address_pool_index_by_sw_if_index, (sw_if_index)) :
1221           (u32) ~0;
1222   return pool_elt_at_index((lm)->if_address_pool, ia);
1223 }
1224
1225 void *
1226 ip_interface_get_first_ip_addres (ip_lookup_main_t *lm, u32 sw_if_index,
1227                                    u8 loop)
1228 {
1229   ip_interface_address_t * ia = ip_interface_get_first_interface_address (
1230       lm, sw_if_index, loop);
1231   return ip_interface_address_get_address (lm, ia);
1232 }
1233
1234 void
1235 del_fwd_entry (lisp_cp_main_t * lcm, u32 src_map_index,
1236                u32 dst_map_index)
1237 {
1238   vnet_lisp_gpe_add_del_fwd_entry_args_t _a, * a = &_a;
1239   fwd_entry_t * fe = 0;
1240   uword * feip = 0;
1241   memset(a, 0, sizeof(*a));
1242
1243   feip = hash_get(lcm->fwd_entry_by_mapping_index, dst_map_index);
1244   if (!feip)
1245     return;
1246
1247   fe = pool_elt_at_index(lcm->fwd_entry_pool, feip[0]);
1248
1249   /* delete dp fwd entry */
1250   u32 sw_if_index;
1251   a->is_add = 0;
1252   a->dlocator = fe->dst_loc;
1253   a->slocator = fe->src_loc;
1254   a->iid = 0; // XXX should be part of mapping/eid
1255   gid_address_copy(&a->deid, &fe->deid);
1256
1257   vnet_lisp_gpe_add_del_fwd_entry (a, &sw_if_index);
1258
1259   /* delete entry in fwd table */
1260   hash_unset(lcm->fwd_entry_by_mapping_index, dst_map_index);
1261   pool_put(lcm->fwd_entry_pool, fe);
1262 }
1263
1264 void
1265 add_fwd_entry (lisp_cp_main_t* lcm, u32 src_map_index, u32 dst_map_index)
1266 {
1267   mapping_t * src_map, * dst_map;
1268   locator_set_t * dst_ls, * src_ls;
1269   u32 i, minp = ~0;
1270   locator_t * dl = 0;
1271   uword * feip = 0;
1272   vnet_lisp_gpe_add_del_fwd_entry_args_t _a, * a = &_a;
1273   memset (a, 0, sizeof(*a));
1274
1275   /* remove entry if it already exists */
1276   feip = hash_get (lcm->fwd_entry_by_mapping_index, dst_map_index);
1277   if (feip)
1278     del_fwd_entry (lcm, src_map_index, dst_map_index);
1279
1280   src_map = pool_elt_at_index (lcm->mapping_pool, src_map_index);
1281   dst_map = pool_elt_at_index (lcm->mapping_pool, dst_map_index);
1282
1283   /* XXX simple forwarding policy: first lowest (value) priority locator */
1284   dst_ls = pool_elt_at_index (lcm->locator_set_pool,
1285                               dst_map->locator_set_index);
1286   for (i = 0; i < vec_len (dst_ls->locator_indices); i++)
1287     {
1288       u32 li = vec_elt (dst_ls->locator_indices, i);
1289       locator_t * l = pool_elt_at_index (lcm->locator_pool, li);
1290       if (l->priority < minp && gid_address_type(&l->address) == IP_PREFIX)
1291         {
1292           minp = l->priority;
1293           dl = l;
1294         }
1295     }
1296   if (dl)
1297     {
1298       src_ls = pool_elt_at_index (lcm->locator_set_pool,
1299                                   src_map->locator_set_index);
1300       for (i = 0; i < vec_len (src_ls->locator_indices); i++)
1301         {
1302           u32 li = vec_elt (src_ls->locator_indices, i);
1303           locator_t * sl = pool_elt_at_index (lcm->locator_pool, li);
1304
1305           if (ip_addr_version(&gid_address_ip(&dl->address)) == IP4)
1306             {
1307               ip4_address_t * l4;
1308               l4 = ip_interface_get_first_ip_addres (&lcm->im4->lookup_main,
1309                                                      sl->sw_if_index,
1310                                                      1 /* unnumbered */);
1311               ip_addr_v4(&a->slocator) = *l4;
1312               ip_addr_version(&a->slocator) = IP4;
1313             }
1314           else
1315             {
1316               ip6_address_t * l6;
1317               l6 = ip_interface_get_first_ip_addres (&lcm->im6->lookup_main,
1318                                                      sl->sw_if_index,
1319                                                      1 /* unnumbered */);
1320               ip_addr_v6(&a->slocator) = *l6;
1321               ip_addr_version(&a->slocator) = IP6;
1322             }
1323         }
1324     }
1325   /* insert data plane forwarding entry */
1326   u32 sw_if_index;
1327   a->is_add = 1;
1328   if (dl)
1329     a->dlocator = gid_address_ip(&dl->address);
1330   else
1331     {
1332       a->is_negative = 1;
1333       a->action = dst_map->action;
1334     }
1335
1336   gid_address_copy (&a->deid, &dst_map->eid);
1337   a->iid = 0; // XXX should be part of mapping/eid
1338   u8 ipver = ip_prefix_version(&gid_address_ippref(&a->deid));
1339   a->decap_next_index = (ipver == IP4) ?
1340           LISP_GPE_INPUT_NEXT_IP4_INPUT : LISP_GPE_INPUT_NEXT_IP6_INPUT;
1341   /* XXX tunnels work only with IP4 now */
1342   vnet_lisp_gpe_add_del_fwd_entry (a, &sw_if_index);
1343
1344   /* add tunnel to fwd entry table XXX check return value from DP insertion */
1345   fwd_entry_t* fe;
1346   pool_get (lcm->fwd_entry_pool, fe);
1347   fe->dst_loc = a->dlocator;
1348   fe->src_loc = a->slocator;
1349   gid_address_copy (&fe->deid, &a->deid);
1350   hash_set (lcm->fwd_entry_by_mapping_index, dst_map_index,
1351             fe - lcm->fwd_entry_pool);
1352 }
1353
1354 /* return 0 if the two locator sets are identical 1 otherwise */
1355 static u8
1356 compare_locators (lisp_cp_main_t *lcm, u32 * old_ls_indexes,
1357                   locator_t * new_locators)
1358 {
1359   u32 i, old_li;
1360   locator_t * old_loc, * new_loc;
1361
1362   if (vec_len (old_ls_indexes) != vec_len(new_locators))
1363     return 1;
1364
1365   for (i = 0; i < vec_len(new_locators); i++)
1366     {
1367       old_li = vec_elt(old_ls_indexes, i);
1368       old_loc = pool_elt_at_index(lcm->locator_pool, old_li);
1369
1370       new_loc = vec_elt_at_index(new_locators, i);
1371
1372       if (locator_cmp (old_loc, new_loc))
1373         return 1;
1374     }
1375   return 0;
1376 }
1377
1378 void
1379 process_map_reply (lisp_cp_main_t * lcm, vlib_buffer_t * b)
1380 {
1381   u32 len = 0, i, ls_index = 0;
1382   void * h;
1383   vnet_lisp_add_del_locator_set_args_t _ls_arg, * ls_arg = &_ls_arg;
1384   vnet_lisp_add_del_mapping_args_t _m_args, * m_args = &_m_args;
1385   pending_map_request_t * pmr;
1386   locator_t probed;
1387   map_reply_hdr_t * mrep_hdr;
1388   u64 nonce;
1389   u32 dst_map_index, mi;
1390   uword * pmr_index;
1391
1392   mrep_hdr = vlib_buffer_get_current (b);
1393
1394   /* Check pending requests table and nonce */
1395   nonce = MREP_NONCE(mrep_hdr);
1396   pmr_index = hash_get(lcm->pending_map_requests_by_nonce, nonce);
1397   if (!pmr_index)
1398     {
1399       clib_warning("No pending map-request entry with nonce %lu!", nonce);
1400       return;
1401     }
1402   pmr = pool_elt_at_index(lcm->pending_map_requests_pool, pmr_index[0]);
1403
1404   vlib_buffer_pull (b, sizeof(*mrep_hdr));
1405
1406   for (i = 0; i < MREP_REC_COUNT(mrep_hdr); i++)
1407     {
1408       memset (ls_arg, 0, sizeof(*ls_arg));
1409       memset (m_args, 0, sizeof(*m_args));
1410
1411       h = vlib_buffer_get_current (b);
1412       m_args->ttl = clib_net_to_host_u32 (MAP_REC_TTL(h));
1413       m_args->action = MAP_REC_ACTION(h);
1414       m_args->authoritative = MAP_REC_AUTH(h);
1415
1416       len = lisp_msg_parse_mapping_record (b, &m_args->deid, &ls_arg->locators,
1417                                            &probed);
1418       if (len == ~0)
1419         {
1420           clib_warning ("Failed to parse mapping record!");
1421           vec_free(ls_arg->locators);
1422           return;
1423         }
1424
1425       mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &m_args->deid);
1426
1427       /* if mapping already exists, decide if locators (and forwarding) should
1428        * be updated and be done */
1429       if (mi != ~0)
1430         {
1431           mapping_t * old_map;
1432           locator_set_t * old_ls;
1433           old_map = pool_elt_at_index(lcm->mapping_pool, mi);
1434
1435           /* update mapping attributes */
1436           old_map->action = m_args->action;
1437           old_map->authoritative = m_args->authoritative;
1438           old_map->ttl = m_args->ttl;
1439
1440           old_ls = pool_elt_at_index(lcm->locator_set_pool,
1441                                      old_map->locator_set_index);
1442           /* if the two locators are not equal, update them and forwarding
1443            * otherwise there's nothing to be done */
1444           if (compare_locators (lcm, old_ls->locator_indices, ls_arg->locators))
1445             {
1446               /* set locator-set index to overwrite */
1447               ls_arg->is_add = 1;
1448               ls_arg->index = old_map->locator_set_index;
1449               vnet_lisp_add_del_locator_set (ls_arg, 0);
1450               add_fwd_entry (lcm, pmr->src_mapping_index, mi);
1451             }
1452         }
1453       /* new mapping */
1454       else
1455         {
1456           /* add locator-set */
1457           ls_arg->is_add = 1;
1458           ls_arg->index = ~0;
1459           vnet_lisp_add_del_locator_set (ls_arg, &ls_index);
1460
1461           /* add mapping */
1462           m_args->is_add = 1;
1463           m_args->locator_set_index = ls_index;
1464           vnet_lisp_add_del_mapping (m_args, &dst_map_index);
1465
1466           /* add forwarding tunnel */
1467           add_fwd_entry (lcm, pmr->src_mapping_index, dst_map_index);
1468         }
1469       vec_free(ls_arg->locators);
1470     }
1471
1472   /* remove pending map request entry */
1473   hash_unset(lcm->pending_map_requests_by_nonce, nonce);
1474   pool_put(lcm->pending_map_requests_pool, pmr);
1475 }
1476
1477 void
1478 process_map_request (vlib_main_t * vm, lisp_cp_main_t * lcm, vlib_buffer_t * b)
1479 {
1480   map_request_hdr_t * mreq_hdr;
1481   gid_address_t src, dst;
1482 //  u64 nonce;
1483   u32 i, len = 0;
1484   gid_address_t * itr_rlocs = 0;
1485
1486   mreq_hdr = vlib_buffer_get_current (b);
1487   vlib_buffer_pull (b, sizeof(*mreq_hdr));
1488
1489 //  nonce = MREQ_NONCE(mreq_hdr);
1490
1491   if (!MREQ_SMR(mreq_hdr)) {
1492       clib_warning("Only SMR Map-Requests supported for now!");
1493       return;
1494   }
1495
1496   /* parse src eid */
1497   len = lisp_msg_parse_addr (b, &src);
1498   if (len == ~0)
1499     return;
1500
1501   /* for now we don't do anything with the itr's rlocs */
1502   len = lisp_msg_parse_itr_rlocs (b, &itr_rlocs, MREQ_ITR_RLOC_COUNT(mreq_hdr) + 1);
1503   if (len == ~0)
1504     return;
1505
1506   /* parse eid records and send SMR-invoked map-requests */
1507   for (i = 0; i < MREQ_REC_COUNT(mreq_hdr); i++)
1508     {
1509       memset(&dst, 0, sizeof(dst));
1510       len = lisp_msg_parse_eid_rec (b, &dst);
1511       if (len == ~0)
1512         {
1513           clib_warning("Can't parse map-request EID-record");
1514           return;
1515         }
1516       /* send SMR-invoked map-requests */
1517       send_encapsulated_map_request (vm, lcm, &dst, &src, /* invoked */ 1);
1518     }
1519 }
1520
1521 static uword
1522 lisp_cp_input (vlib_main_t * vm, vlib_node_runtime_t * node,
1523                vlib_frame_t * from_frame)
1524 {
1525   u32 n_left_from, * from, * to_next_drop;
1526   lisp_msg_type_e type;
1527   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
1528
1529   from = vlib_frame_vector_args (from_frame);
1530   n_left_from = from_frame->n_vectors;
1531
1532
1533   while (n_left_from > 0)
1534     {
1535       u32 n_left_to_next_drop;
1536
1537       vlib_get_next_frame (vm, node, LISP_CP_INPUT_NEXT_DROP,
1538                            to_next_drop, n_left_to_next_drop);
1539       while (n_left_from > 0 && n_left_to_next_drop > 0)
1540         {
1541           u32 bi0;
1542           vlib_buffer_t * b0;
1543
1544           bi0 = from[0];
1545           from += 1;
1546           n_left_from -= 1;
1547           to_next_drop[0] = bi0;
1548           to_next_drop += 1;
1549           n_left_to_next_drop -= 1;
1550
1551           b0 = vlib_get_buffer (vm, bi0);
1552
1553           type = lisp_msg_type(vlib_buffer_get_current (b0));
1554           switch (type)
1555             {
1556             case LISP_MAP_REPLY:
1557               process_map_reply (lcm, b0);
1558               break;
1559             case LISP_MAP_REQUEST:
1560               process_map_request(vm, lcm, b0);
1561               break;
1562             default:
1563               clib_warning("Unsupported LISP message type %d", type);
1564               break;
1565             }
1566
1567           b0->error = node->errors[LISP_CP_INPUT_ERROR_DROP];
1568
1569           if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED))
1570             {
1571
1572             }
1573         }
1574
1575       vlib_put_next_frame (vm, node, LISP_CP_INPUT_NEXT_DROP, n_left_to_next_drop);
1576     }
1577   return from_frame->n_vectors;
1578 }
1579
1580 VLIB_REGISTER_NODE (lisp_cp_input_node) = {
1581   .function = lisp_cp_input,
1582   .name = "lisp-cp-input",
1583   .vector_size = sizeof (u32),
1584   .format_trace = format_lisp_cp_input_trace,
1585   .type = VLIB_NODE_TYPE_INTERNAL,
1586
1587   .n_errors = LISP_CP_INPUT_N_ERROR,
1588   .error_strings = lisp_cp_input_error_strings,
1589
1590   .n_next_nodes = LISP_CP_INPUT_N_NEXT,
1591
1592   .next_nodes = {
1593       [LISP_CP_INPUT_NEXT_DROP] = "error-drop",
1594   },
1595 };
1596
1597 clib_error_t *
1598 lisp_cp_init (vlib_main_t *vm)
1599 {
1600   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
1601   clib_error_t * error = 0;
1602
1603   if ((error = vlib_call_init_function (vm, lisp_gpe_init)))
1604     return error;
1605
1606   lcm->im4 = &ip4_main;
1607   lcm->im6 = &ip6_main;
1608   lcm->vlib_main = vm;
1609   lcm->vnet_main = vnet_get_main();
1610
1611   gid_dictionary_init (&lcm->mapping_index_by_gid);
1612   gid_dictionary_init (&lcm->mapping_index_by_gid);
1613
1614   udp_register_dst_port (vm, UDP_DST_PORT_lisp_cp,
1615                          lisp_cp_input_node.index, 1 /* is_ip4 */);
1616   udp_register_dst_port (vm, UDP_DST_PORT_lisp_cp6,
1617                          lisp_cp_input_node.index, 0 /* is_ip4 */);
1618
1619   return 0;
1620 }
1621
1622 VLIB_INIT_FUNCTION(lisp_cp_init);