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_mr_and_local_iface_ip (lisp_cp_main_t *lcm, ip_address_t * mr_ip,
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   ip_address_t * mrit;
1045
1046   if (vec_len(lcm->map_resolvers) == 0)
1047     {
1048       clib_warning("No map-resolver configured");
1049       return;
1050     }
1051
1052   /* find the first mr ip we have a route to and the ip of the
1053    * iface that has a route to it */
1054   vec_foreach(mrit, lcm->map_resolvers)
1055     {
1056       lm = ip_addr_version (mrit) == IP4 ?
1057           &lcm->im4->lookup_main : &lcm->im6->lookup_main;
1058
1059       adj_index = ip_fib_lookup_with_table (lcm, 0, mrit);
1060       adj = ip_get_adjacency (lm, adj_index);
1061
1062       if (adj == 0)
1063         continue;
1064
1065       if (adj->lookup_next_index == IP_LOOKUP_NEXT_ARP)
1066         {
1067           ia = pool_elt_at_index(lm->if_address_pool, adj->if_address_index);
1068           if (ip_addr_version(mrit) == IP4)
1069             {
1070               l4 = ip_interface_address_get_address (lm, ia);
1071             }
1072           else
1073             {
1074               l6 = ip_interface_address_get_address (lm, ia);
1075             }
1076         }
1077       else if (adj->lookup_next_index == IP_LOOKUP_NEXT_REWRITE)
1078         {
1079           /* find sw_if_index in rewrite header */
1080           u32 sw_if_index = adj->rewrite_header.sw_if_index;
1081
1082           /* find suitable address */
1083           if (ip_addr_version(mrit) == IP4)
1084             {
1085               /* find the first ip address */
1086               foreach_ip_interface_address (&lcm->im4->lookup_main, ia,
1087                                             sw_if_index, 1 /* unnumbered */,
1088               ({
1089                 l4 = ip_interface_address_get_address (&lcm->im4->lookup_main,
1090                                                        ia);
1091                 break;
1092               }));
1093             }
1094           else
1095             {
1096               /* find the first ip address */
1097               foreach_ip_interface_address (&lcm->im6->lookup_main, ia,
1098                                             sw_if_index, 1 /* unnumbered */,
1099               ({
1100                 l6 = ip_interface_address_get_address (&lcm->im6->lookup_main,
1101                                                        ia);
1102                 break;
1103               }));
1104             }
1105         }
1106
1107       if (l4)
1108         {
1109           ip_addr_v4(sloc).as_u32 = l4->as_u32;
1110           ip_addr_version(sloc) = IP4;
1111           ip_address_copy(mr_ip, mrit);
1112           return;
1113         }
1114       else if (l6)
1115         {
1116           clib_memcpy (&ip_addr_v6(sloc), l6, sizeof(*l6));
1117           ip_addr_version(sloc) = IP6;
1118           ip_address_copy(mr_ip, mrit);
1119           return;
1120         }
1121     }
1122
1123   clib_warning("Can't find map-resolver and local interface ip!");
1124   return;
1125 }
1126
1127 static gid_address_t *
1128 build_itr_rloc_list (lisp_cp_main_t * lcm, locator_set_t * loc_set)
1129 {
1130   ip4_address_t * l4;
1131   ip6_address_t * l6;
1132   u32 i;
1133   locator_t * loc;
1134   u32 * loc_indexp;
1135   ip_interface_address_t * ia = 0;
1136   gid_address_t gid_data, * gid = &gid_data;
1137   gid_address_t * rlocs = 0;
1138   ip_prefix_t * ippref = &gid_address_ippref (gid);
1139   ip_address_t * rloc = &ip_prefix_addr (ippref);
1140
1141   gid_address_type (gid) = GID_ADDR_IP_PREFIX;
1142   for (i = 0; i < vec_len(loc_set->locator_indices); i++)
1143     {
1144       loc_indexp = vec_elt_at_index(loc_set->locator_indices, i);
1145       loc = pool_elt_at_index (lcm->locator_pool, loc_indexp[0]);
1146
1147       ip_addr_version(rloc) = IP4;
1148       /* Add ipv4 locators first TODO sort them */
1149       foreach_ip_interface_address (&lcm->im4->lookup_main, ia,
1150                                     loc->sw_if_index, 1 /* unnumbered */,
1151       ({
1152         l4 = ip_interface_address_get_address (&lcm->im4->lookup_main, ia);
1153         ip_addr_v4 (rloc) = l4[0];
1154         ip_prefix_len (ippref) = 32;
1155         vec_add1 (rlocs, gid[0]);
1156       }));
1157
1158       ip_addr_version(rloc) = IP6;
1159       /* Add ipv6 locators */
1160       foreach_ip_interface_address (&lcm->im6->lookup_main, ia,
1161                                     loc->sw_if_index, 1 /* unnumbered */,
1162       ({
1163         l6 = ip_interface_address_get_address (&lcm->im6->lookup_main, ia);
1164         ip_addr_v6 (rloc) = l6[0];
1165         ip_prefix_len (ippref) = 128;
1166         vec_add1 (rlocs, gid[0]);
1167       }));
1168     }
1169   return rlocs;
1170 }
1171
1172 static vlib_buffer_t *
1173 build_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
1174                                 gid_address_t * seid, gid_address_t * deid,
1175                                 locator_set_t * loc_set, ip_address_t * mr_ip,
1176                                 ip_address_t * sloc, u8 is_smr_invoked,
1177                                 u64 *nonce_res, u32 * bi_res)
1178 {
1179   vlib_buffer_t * b;
1180   u32 bi;
1181   gid_address_t * rlocs = 0;
1182
1183   if (vlib_buffer_alloc (vm, &bi, 1) != 1)
1184     {
1185       clib_warning ("Can't allocate buffer for Map-Request!");
1186       return 0;
1187     }
1188
1189   b = vlib_get_buffer (vm, bi);
1190
1191   /* leave some space for the encap headers */
1192   vlib_buffer_make_headroom (b, MAX_LISP_MSG_ENCAP_LEN);
1193
1194   /* get rlocs */
1195   rlocs = build_itr_rloc_list (lcm, loc_set);
1196
1197   /* put lisp msg */
1198   lisp_msg_put_mreq (lcm, b, seid, deid, rlocs, is_smr_invoked, nonce_res);
1199
1200   /* push ecm: udp-ip-lisp */
1201   lisp_msg_push_ecm (vm, b, LISP_CONTROL_PORT, LISP_CONTROL_PORT, seid, deid);
1202
1203   /* push outer ip header */
1204   pkt_push_udp_and_ip (vm, b, LISP_CONTROL_PORT, LISP_CONTROL_PORT, sloc,
1205                        mr_ip);
1206
1207   bi_res[0] = bi;
1208
1209   if (rlocs)
1210     vec_free(rlocs);
1211   return b;
1212 }
1213
1214 static void
1215 send_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
1216                                gid_address_t * seid, gid_address_t * deid,
1217                                u8 is_smr_invoked)
1218 {
1219   u32 next_index, bi = 0, * to_next, map_index;
1220   vlib_buffer_t * b;
1221   vlib_frame_t * f;
1222   u64 nonce = 0;
1223   locator_set_t * loc_set;
1224   mapping_t * map;
1225   pending_map_request_t * pmr;
1226   ip_address_t mr_ip, sloc;
1227
1228   /* get locator-set for seid */
1229   map_index = gid_dictionary_lookup (&lcm->mapping_index_by_gid, seid);
1230   if (map_index == ~0)
1231     {
1232       clib_warning("No local mapping found in eid-table for %U!",
1233                    format_gid_address, seid);
1234       return;
1235     }
1236
1237   map = pool_elt_at_index (lcm->mapping_pool, map_index);
1238
1239   if (!map->local)
1240     {
1241       clib_warning("Mapping found for src eid %U is not marked as local!",
1242                    format_gid_address, seid);
1243       return;
1244     }
1245   loc_set = pool_elt_at_index (lcm->locator_set_pool, map->locator_set_index);
1246
1247   /* get local iface ip to use in map-request XXX fib 0 for now*/
1248   get_mr_and_local_iface_ip (lcm, &mr_ip, &sloc);
1249
1250   /* build the encapsulated map request */
1251   b = build_encapsulated_map_request (vm, lcm, seid, deid, loc_set, &mr_ip,
1252                                       &sloc, is_smr_invoked, &nonce, &bi);
1253
1254   if (!b)
1255     return;
1256
1257   /* set fib index and lookup node */
1258   vnet_buffer(b)->sw_if_index[VLIB_TX] = ~0;
1259   next_index = (ip_addr_version(&mr_ip) == IP4) ?
1260       ip4_lookup_node.index : ip6_lookup_node.index;
1261
1262   f = vlib_get_frame_to_node (vm, next_index);
1263
1264   /* Enqueue the packet */
1265   to_next = vlib_frame_vector_args (f);
1266   to_next[0] = bi;
1267   f->n_vectors = 1;
1268   vlib_put_frame_to_node (vm, next_index, f);
1269
1270   /* add map-request to pending requests table */
1271   pool_get(lcm->pending_map_requests_pool, pmr);
1272   gid_address_copy (&pmr->src, seid);
1273   gid_address_copy (&pmr->dst, deid);
1274   pmr->src_mapping_index = map_index;
1275   hash_set(lcm->pending_map_requests_by_nonce, nonce,
1276            pmr - lcm->pending_map_requests_pool);
1277 }
1278
1279 static void
1280 get_src_and_dst (void *hdr, ip_address_t * src, ip_address_t *dst)
1281 {
1282   ip4_header_t * ip4 = hdr;
1283   ip6_header_t * ip6;
1284
1285   if ((ip4->ip_version_and_header_length & 0xF0) == 0x40)
1286     {
1287       ip_addr_v4(src).as_u32 = ip4->src_address.as_u32;
1288       ip_addr_version(src) = IP4;
1289       ip_addr_v4(dst).as_u32 = ip4->dst_address.as_u32;
1290       ip_addr_version(dst) = IP4;
1291     }
1292   else
1293     {
1294       ip6 = hdr;
1295       clib_memcpy (&ip_addr_v6(src), &ip6->src_address, sizeof(ip6->src_address));
1296       ip_addr_version(src) = IP6;
1297       clib_memcpy (&ip_addr_v6(dst), &ip6->dst_address, sizeof(ip6->dst_address));
1298       ip_addr_version(dst) = IP6;
1299     }
1300 }
1301
1302 static uword
1303 lisp_cp_lookup (vlib_main_t * vm, vlib_node_runtime_t * node,
1304               vlib_frame_t * from_frame)
1305 {
1306   u32 * from, * to_next_drop, di, si;
1307   lisp_cp_main_t *lcm = vnet_lisp_cp_get_main();
1308   u32 pkts_mapped = 0;
1309   uword n_left_from, n_left_to_next_drop;
1310
1311   from = vlib_frame_vector_args (from_frame);
1312   n_left_from = from_frame->n_vectors;
1313
1314   while (n_left_from > 0)
1315     {
1316       vlib_get_next_frame (vm, node, LISP_CP_LOOKUP_NEXT_DROP,
1317                            to_next_drop, n_left_to_next_drop);
1318
1319       while (n_left_from > 0 && n_left_to_next_drop > 0)
1320         {
1321           u32 pi0;
1322           vlib_buffer_t * p0;
1323           ip4_header_t * ip0;
1324           gid_address_t src, dst;
1325           ip_prefix_t * spref, * dpref;
1326
1327           gid_address_type (&src) = GID_ADDR_IP_PREFIX;
1328           spref = &gid_address_ippref(&src);
1329           gid_address_type (&dst) = GID_ADDR_IP_PREFIX;
1330           dpref = &gid_address_ippref(&dst);
1331
1332           pi0 = from[0];
1333           from += 1;
1334           n_left_from -= 1;
1335           to_next_drop[0] = pi0;
1336           to_next_drop += 1;
1337           n_left_to_next_drop -= 1;
1338
1339           p0 = vlib_get_buffer (vm, pi0);
1340           p0->error = node->errors[LISP_CP_LOOKUP_ERROR_DROP];
1341
1342           /* src/dst eid pair */
1343           ip0 = vlib_buffer_get_current (p0);
1344           get_src_and_dst (ip0, &ip_prefix_addr(spref), &ip_prefix_addr(dpref));
1345           ip_prefix_len(spref) = ip_address_max_len (ip_prefix_version(spref));
1346           ip_prefix_len(dpref) = ip_address_max_len (ip_prefix_version(dpref));
1347
1348           /* if we have remote mapping for destination already in map-chache
1349              add forwarding tunnel directly. If not send a map-request */
1350           di = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &dst);
1351           if (~0 != di)
1352             {
1353               si =  gid_dictionary_lookup (&lcm->mapping_index_by_gid, &src);
1354               if (~0 != si)
1355                 {
1356                   add_fwd_entry (lcm, si, di);
1357                 }
1358             }
1359           else
1360             {
1361               /* send map-request */
1362               send_encapsulated_map_request (vm, lcm, &src, &dst, 0);
1363               pkts_mapped++;
1364             }
1365
1366           if (PREDICT_FALSE(p0->flags & VLIB_BUFFER_IS_TRACED))
1367             {
1368               lisp_cp_lookup_trace_t *tr = vlib_add_trace (vm, node, p0,
1369                                                           sizeof(*tr));
1370               gid_address_copy (&tr->dst_eid, &dst);
1371               clib_memcpy (&tr->map_resolver_ip,
1372                       vec_elt_at_index(lcm->map_resolvers, 0),
1373                       sizeof(ip_address_t));
1374             }
1375         }
1376
1377       vlib_put_next_frame (vm, node, LISP_CP_LOOKUP_NEXT_DROP, n_left_to_next_drop);
1378     }
1379   vlib_node_increment_counter (vm, node->node_index,
1380                                LISP_CP_LOOKUP_ERROR_MAP_REQUESTS_SENT,
1381                                pkts_mapped);
1382   return from_frame->n_vectors;
1383 }
1384
1385 VLIB_REGISTER_NODE (lisp_cp_lookup_node) = {
1386   .function = lisp_cp_lookup,
1387   .name = "lisp-cp-lookup",
1388   .vector_size = sizeof (u32),
1389   .format_trace = format_lisp_cp_lookup_trace,
1390   .type = VLIB_NODE_TYPE_INTERNAL,
1391
1392   .n_errors = LISP_CP_LOOKUP_N_ERROR,
1393   .error_strings = lisp_cp_lookup_error_strings,
1394
1395   .n_next_nodes = LISP_CP_LOOKUP_N_NEXT,
1396
1397   .next_nodes = {
1398       [LISP_CP_LOOKUP_NEXT_DROP] = "error-drop",
1399       [LISP_CP_LOOKUP_NEXT_IP4_LOOKUP] = "ip4-lookup",
1400       [LISP_CP_LOOKUP_NEXT_IP6_LOOKUP] = "ip6-lookup",
1401   },
1402 };
1403
1404 /* lisp_cp_input statistics */
1405 #define foreach_lisp_cp_input_error                   \
1406 _(DROP, "drop")                                        \
1407 _(MAP_REPLIES_RECEIVED, "map-replies received")
1408
1409 static char * lisp_cp_input_error_strings[] = {
1410 #define _(sym,string) string,
1411   foreach_lisp_cp_input_error
1412 #undef _
1413 };
1414
1415 typedef enum
1416 {
1417 #define _(sym,str) LISP_CP_INPUT_ERROR_##sym,
1418     foreach_lisp_cp_input_error
1419 #undef _
1420     LISP_CP_INPUT_N_ERROR,
1421 } lisp_cp_input_error_t;
1422
1423 typedef enum
1424 {
1425   LISP_CP_INPUT_NEXT_DROP,
1426   LISP_CP_INPUT_N_NEXT,
1427 } lisp_cp_input_next_t;
1428
1429 typedef struct
1430 {
1431   gid_address_t dst_eid;
1432   ip4_address_t map_resolver_ip;
1433 } lisp_cp_input_trace_t;
1434
1435 u8 *
1436 format_lisp_cp_input_trace (u8 * s, va_list * args)
1437 {
1438   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
1439   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
1440   CLIB_UNUSED(lisp_cp_input_trace_t * t) = va_arg (*args, lisp_cp_input_trace_t *);
1441
1442   s = format (s, "LISP-CP-INPUT: TODO");
1443   return s;
1444 }
1445
1446 ip_interface_address_t *
1447 ip_interface_get_first_interface_address (ip_lookup_main_t *lm, u32 sw_if_index,
1448                                           u8 loop)
1449 {
1450   vnet_main_t *vnm = vnet_get_main ();
1451   vnet_sw_interface_t * swif = vnet_get_sw_interface (vnm, sw_if_index);
1452   if (loop && swif->flags & VNET_SW_INTERFACE_FLAG_UNNUMBERED)
1453     sw_if_index = swif->unnumbered_sw_if_index;
1454   u32 ia =
1455       (vec_len((lm)->if_address_pool_index_by_sw_if_index) > (sw_if_index)) ?
1456           vec_elt((lm)->if_address_pool_index_by_sw_if_index, (sw_if_index)) :
1457           (u32) ~0;
1458   return pool_elt_at_index((lm)->if_address_pool, ia);
1459 }
1460
1461 void *
1462 ip_interface_get_first_ip_addres (ip_lookup_main_t *lm, u32 sw_if_index,
1463                                    u8 loop)
1464 {
1465   ip_interface_address_t * ia = ip_interface_get_first_interface_address (
1466       lm, sw_if_index, loop);
1467   return ip_interface_address_get_address (lm, ia);
1468 }
1469
1470 void
1471 del_fwd_entry (lisp_cp_main_t * lcm, u32 src_map_index,
1472                u32 dst_map_index)
1473 {
1474   vnet_lisp_gpe_add_del_fwd_entry_args_t _a, * a = &_a;
1475   fwd_entry_t * fe = 0;
1476   uword * feip = 0;
1477   memset(a, 0, sizeof(*a));
1478
1479   feip = hash_get(lcm->fwd_entry_by_mapping_index, dst_map_index);
1480   if (!feip)
1481     return;
1482
1483   fe = pool_elt_at_index(lcm->fwd_entry_pool, feip[0]);
1484
1485   /* delete dp fwd entry */
1486   u32 sw_if_index;
1487   a->is_add = 0;
1488   a->dlocator = fe->dst_loc;
1489   a->slocator = fe->src_loc;
1490   a->vni = gid_address_vni(&a->deid);
1491   gid_address_copy(&a->deid, &fe->deid);
1492
1493   vnet_lisp_gpe_add_del_fwd_entry (a, &sw_if_index);
1494
1495   /* delete entry in fwd table */
1496   hash_unset(lcm->fwd_entry_by_mapping_index, dst_map_index);
1497   pool_put(lcm->fwd_entry_pool, fe);
1498 }
1499
1500 static void
1501 add_fwd_entry (lisp_cp_main_t* lcm, u32 src_map_index, u32 dst_map_index)
1502 {
1503   mapping_t * src_map, * dst_map;
1504   locator_set_t * dst_ls, * src_ls;
1505   u32 i, minp = ~0, sw_if_index;
1506   locator_t * dl = 0;
1507   uword * feip = 0, * tidp;
1508   fwd_entry_t* fe;
1509   vnet_lisp_gpe_add_del_fwd_entry_args_t _a, * a = &_a;
1510
1511   memset (a, 0, sizeof(*a));
1512
1513   /* remove entry if it already exists */
1514   feip = hash_get (lcm->fwd_entry_by_mapping_index, dst_map_index);
1515   if (feip)
1516     del_fwd_entry (lcm, src_map_index, dst_map_index);
1517
1518   src_map = pool_elt_at_index (lcm->mapping_pool, src_map_index);
1519   dst_map = pool_elt_at_index (lcm->mapping_pool, dst_map_index);
1520
1521   gid_address_copy (&a->deid, &dst_map->eid);
1522   a->vni = gid_address_vni(&a->deid);
1523
1524   tidp = hash_get(lcm->table_id_by_vni, a->vni);
1525   if (!tidp)
1526     {
1527       clib_warning("vni %d not associated to a vrf!", a->vni);
1528       return;
1529     }
1530   a->table_id = tidp[0];
1531
1532   /* XXX simple forwarding policy: first lowest (value) priority locator */
1533   dst_ls = pool_elt_at_index (lcm->locator_set_pool,
1534                               dst_map->locator_set_index);
1535   for (i = 0; i < vec_len (dst_ls->locator_indices); i++)
1536     {
1537       u32 li = vec_elt (dst_ls->locator_indices, i);
1538       locator_t * l = pool_elt_at_index (lcm->locator_pool, li);
1539       if (l->priority < minp && gid_address_type(&l->address)
1540             == GID_ADDR_IP_PREFIX)
1541         {
1542           minp = l->priority;
1543           dl = l;
1544         }
1545     }
1546   if (dl)
1547     {
1548       src_ls = pool_elt_at_index(lcm->locator_set_pool,
1549                                  src_map->locator_set_index);
1550       for (i = 0; i < vec_len(src_ls->locator_indices); i++)
1551         {
1552           u32 li = vec_elt (src_ls->locator_indices, i);
1553           locator_t * sl = pool_elt_at_index (lcm->locator_pool, li);
1554
1555           if (ip_addr_version(&gid_address_ip(&dl->address)) == IP4)
1556             {
1557               ip4_address_t * l4;
1558               l4 = ip_interface_get_first_ip_addres (&lcm->im4->lookup_main,
1559                                                      sl->sw_if_index,
1560                                                      1 /* unnumbered */);
1561               ip_addr_v4(&a->slocator) = *l4;
1562               ip_addr_version(&a->slocator) = IP4;
1563             }
1564           else
1565             {
1566               ip6_address_t * l6;
1567               l6 = ip_interface_get_first_ip_addres (&lcm->im6->lookup_main,
1568                                                      sl->sw_if_index,
1569                                                      1 /* unnumbered */);
1570               ip_addr_v6(&a->slocator) = *l6;
1571               ip_addr_version(&a->slocator) = IP6;
1572             }
1573         }
1574     }
1575
1576   /* insert data plane forwarding entry */
1577   a->is_add = 1;
1578   if (dl)
1579     a->dlocator = gid_address_ip(&dl->address);
1580   else
1581     {
1582       a->is_negative = 1;
1583       a->action = dst_map->action;
1584     }
1585
1586   /* TODO remove */
1587   u8 ipver = ip_prefix_version(&gid_address_ippref(&a->deid));
1588   a->decap_next_index = (ipver == IP4) ?
1589           LISP_GPE_INPUT_NEXT_IP4_INPUT : LISP_GPE_INPUT_NEXT_IP6_INPUT;
1590
1591   vnet_lisp_gpe_add_del_fwd_entry (a, &sw_if_index);
1592
1593   /* add tunnel to fwd entry table XXX check return value from DP insertion */
1594   pool_get (lcm->fwd_entry_pool, fe);
1595   fe->dst_loc = a->dlocator;
1596   fe->src_loc = a->slocator;
1597   gid_address_copy (&fe->deid, &a->deid);
1598   hash_set (lcm->fwd_entry_by_mapping_index, dst_map_index,
1599             fe - lcm->fwd_entry_pool);
1600 }
1601
1602 /* return 0 if the two locator sets are identical 1 otherwise */
1603 static u8
1604 compare_locators (lisp_cp_main_t *lcm, u32 * old_ls_indexes,
1605                   locator_t * new_locators)
1606 {
1607   u32 i, old_li;
1608   locator_t * old_loc, * new_loc;
1609
1610   if (vec_len (old_ls_indexes) != vec_len(new_locators))
1611     return 1;
1612
1613   for (i = 0; i < vec_len(new_locators); i++)
1614     {
1615       old_li = vec_elt(old_ls_indexes, i);
1616       old_loc = pool_elt_at_index(lcm->locator_pool, old_li);
1617
1618       new_loc = vec_elt_at_index(new_locators, i);
1619
1620       if (locator_cmp (old_loc, new_loc))
1621         return 1;
1622     }
1623   return 0;
1624 }
1625
1626 void
1627 process_map_reply (lisp_cp_main_t * lcm, vlib_buffer_t * b)
1628 {
1629   locator_t * loc;
1630   u32 len = 0, i, ls_index = 0;
1631   void * h;
1632   vnet_lisp_add_del_locator_set_args_t _ls_arg, * ls_arg = &_ls_arg;
1633   vnet_lisp_add_del_mapping_args_t _m_args, * m_args = &_m_args;
1634   pending_map_request_t * pmr;
1635   locator_t probed;
1636   map_reply_hdr_t * mrep_hdr;
1637   u64 nonce;
1638   u32 dst_map_index, mi;
1639   uword * pmr_index;
1640
1641   mrep_hdr = vlib_buffer_get_current (b);
1642
1643   /* Check pending requests table and nonce */
1644   nonce = MREP_NONCE(mrep_hdr);
1645   pmr_index = hash_get(lcm->pending_map_requests_by_nonce, nonce);
1646   if (!pmr_index)
1647     {
1648       clib_warning("No pending map-request entry with nonce %lu!", nonce);
1649       return;
1650     }
1651   pmr = pool_elt_at_index(lcm->pending_map_requests_pool, pmr_index[0]);
1652
1653   vlib_buffer_pull (b, sizeof(*mrep_hdr));
1654
1655   for (i = 0; i < MREP_REC_COUNT(mrep_hdr); i++)
1656     {
1657       memset (ls_arg, 0, sizeof(*ls_arg));
1658       memset (m_args, 0, sizeof(*m_args));
1659
1660       h = vlib_buffer_get_current (b);
1661       m_args->ttl = clib_net_to_host_u32 (MAP_REC_TTL(h));
1662       m_args->action = MAP_REC_ACTION(h);
1663       m_args->authoritative = MAP_REC_AUTH(h);
1664
1665       len = lisp_msg_parse_mapping_record (b, &m_args->deid, &ls_arg->locators,
1666                                            &probed);
1667       if (len == ~0)
1668         {
1669           clib_warning ("Failed to parse mapping record!");
1670           vec_foreach (loc, ls_arg->locators)
1671             {
1672               locator_free (loc);
1673             }
1674           vec_free(ls_arg->locators);
1675           return;
1676         }
1677
1678       mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &m_args->deid);
1679
1680       /* if mapping already exists, decide if locators (and forwarding) should
1681        * be updated and be done */
1682       if (mi != ~0)
1683         {
1684           mapping_t * old_map;
1685           locator_set_t * old_ls;
1686           old_map = pool_elt_at_index(lcm->mapping_pool, mi);
1687
1688           /* update mapping attributes */
1689           old_map->action = m_args->action;
1690           old_map->authoritative = m_args->authoritative;
1691           old_map->ttl = m_args->ttl;
1692
1693           old_ls = pool_elt_at_index(lcm->locator_set_pool,
1694                                      old_map->locator_set_index);
1695           /* if the two locators are not equal, update them and forwarding
1696            * otherwise there's nothing to be done */
1697           if (compare_locators (lcm, old_ls->locator_indices, ls_arg->locators))
1698             {
1699               /* set locator-set index to overwrite */
1700               ls_arg->is_add = 1;
1701               ls_arg->index = old_map->locator_set_index;
1702               vnet_lisp_add_del_locator_set (ls_arg, 0);
1703               add_fwd_entry (lcm, pmr->src_mapping_index, mi);
1704             }
1705         }
1706       /* new mapping */
1707       else
1708         {
1709           /* add locator-set */
1710           ls_arg->is_add = 1;
1711           ls_arg->index = ~0;
1712           vnet_lisp_add_del_locator_set (ls_arg, &ls_index);
1713
1714           /* add mapping */
1715           m_args->is_add = 1;
1716           m_args->locator_set_index = ls_index;
1717           vnet_lisp_add_del_mapping (m_args, &dst_map_index);
1718
1719           /* add forwarding tunnel */
1720           add_fwd_entry (lcm, pmr->src_mapping_index, dst_map_index);
1721         }
1722       vec_free(ls_arg->locators);
1723     }
1724
1725   /* remove pending map request entry */
1726   hash_unset(lcm->pending_map_requests_by_nonce, nonce);
1727   pool_put(lcm->pending_map_requests_pool, pmr);
1728 }
1729
1730 void
1731 process_map_request (vlib_main_t * vm, lisp_cp_main_t * lcm, vlib_buffer_t * b)
1732 {
1733   map_request_hdr_t * mreq_hdr;
1734   gid_address_t src, dst;
1735 //  u64 nonce;
1736   u32 i, len = 0;
1737   gid_address_t * itr_rlocs = 0, * rloc;
1738
1739   mreq_hdr = vlib_buffer_get_current (b);
1740   vlib_buffer_pull (b, sizeof(*mreq_hdr));
1741
1742 //  nonce = MREQ_NONCE(mreq_hdr);
1743
1744   if (!MREQ_SMR(mreq_hdr)) {
1745       clib_warning("Only SMR Map-Requests supported for now!");
1746       return;
1747   }
1748
1749   /* parse src eid */
1750   len = lisp_msg_parse_addr (b, &src);
1751   if (len == ~0)
1752     return;
1753
1754   /* for now we don't do anything with the itr's rlocs */
1755   len = lisp_msg_parse_itr_rlocs (b, &itr_rlocs, MREQ_ITR_RLOC_COUNT(mreq_hdr) + 1);
1756   if (len == ~0)
1757     return;
1758
1759   /* TODO: RLOCs are currently unused, so free them for now */
1760   vec_foreach (rloc, itr_rlocs)
1761     {
1762       gid_address_free (rloc);
1763     }
1764
1765   /* parse eid records and send SMR-invoked map-requests */
1766   for (i = 0; i < MREQ_REC_COUNT(mreq_hdr); i++)
1767     {
1768       memset(&dst, 0, sizeof(dst));
1769       len = lisp_msg_parse_eid_rec (b, &dst);
1770       if (len == ~0)
1771         {
1772           clib_warning("Can't parse map-request EID-record");
1773           return;
1774         }
1775       /* send SMR-invoked map-requests */
1776       send_encapsulated_map_request (vm, lcm, &dst, &src, /* invoked */ 1);
1777     }
1778 }
1779
1780 static uword
1781 lisp_cp_input (vlib_main_t * vm, vlib_node_runtime_t * node,
1782                vlib_frame_t * from_frame)
1783 {
1784   u32 n_left_from, * from, * to_next_drop;
1785   lisp_msg_type_e type;
1786   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
1787
1788   from = vlib_frame_vector_args (from_frame);
1789   n_left_from = from_frame->n_vectors;
1790
1791
1792   while (n_left_from > 0)
1793     {
1794       u32 n_left_to_next_drop;
1795
1796       vlib_get_next_frame (vm, node, LISP_CP_INPUT_NEXT_DROP,
1797                            to_next_drop, n_left_to_next_drop);
1798       while (n_left_from > 0 && n_left_to_next_drop > 0)
1799         {
1800           u32 bi0;
1801           vlib_buffer_t * b0;
1802
1803           bi0 = from[0];
1804           from += 1;
1805           n_left_from -= 1;
1806           to_next_drop[0] = bi0;
1807           to_next_drop += 1;
1808           n_left_to_next_drop -= 1;
1809
1810           b0 = vlib_get_buffer (vm, bi0);
1811
1812           type = lisp_msg_type(vlib_buffer_get_current (b0));
1813           switch (type)
1814             {
1815             case LISP_MAP_REPLY:
1816               process_map_reply (lcm, b0);
1817               break;
1818             case LISP_MAP_REQUEST:
1819               process_map_request(vm, lcm, b0);
1820               break;
1821             default:
1822               clib_warning("Unsupported LISP message type %d", type);
1823               break;
1824             }
1825
1826           b0->error = node->errors[LISP_CP_INPUT_ERROR_DROP];
1827
1828           if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED))
1829             {
1830
1831             }
1832         }
1833
1834       vlib_put_next_frame (vm, node, LISP_CP_INPUT_NEXT_DROP, n_left_to_next_drop);
1835     }
1836   return from_frame->n_vectors;
1837 }
1838
1839 VLIB_REGISTER_NODE (lisp_cp_input_node) = {
1840   .function = lisp_cp_input,
1841   .name = "lisp-cp-input",
1842   .vector_size = sizeof (u32),
1843   .format_trace = format_lisp_cp_input_trace,
1844   .type = VLIB_NODE_TYPE_INTERNAL,
1845
1846   .n_errors = LISP_CP_INPUT_N_ERROR,
1847   .error_strings = lisp_cp_input_error_strings,
1848
1849   .n_next_nodes = LISP_CP_INPUT_N_NEXT,
1850
1851   .next_nodes = {
1852       [LISP_CP_INPUT_NEXT_DROP] = "error-drop",
1853   },
1854 };
1855
1856 clib_error_t *
1857 lisp_cp_init (vlib_main_t *vm)
1858 {
1859   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
1860   clib_error_t * error = 0;
1861
1862   if ((error = vlib_call_init_function (vm, lisp_gpe_init)))
1863     return error;
1864
1865   lcm->im4 = &ip4_main;
1866   lcm->im6 = &ip6_main;
1867   lcm->vlib_main = vm;
1868   lcm->vnet_main = vnet_get_main();
1869
1870   gid_dictionary_init (&lcm->mapping_index_by_gid);
1871
1872   /* default vrf mapped to vni 0 */
1873   hash_set(lcm->table_id_by_vni, 0, 0);
1874
1875   udp_register_dst_port (vm, UDP_DST_PORT_lisp_cp,
1876                          lisp_cp_input_node.index, 1 /* is_ip4 */);
1877   udp_register_dst_port (vm, UDP_DST_PORT_lisp_cp6,
1878                          lisp_cp_input_node.index, 0 /* is_ip4 */);
1879
1880   return 0;
1881 }
1882
1883 VLIB_INIT_FUNCTION(lisp_cp_init);