vnet/vnet/lisp-cp/control.c

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