vnet/vnet/lisp-cp/control.c

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