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