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