src/vnet/lisp-cp/lisp_types.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/lisp_types.h>
  17
  18 static u16 gid_address_put_no_vni (u8 * b, gid_address_t * gid);
  19 static u16 gid_address_size_to_put_no_vni (gid_address_t * gid);
  20 static u16 fid_addr_size_to_write (fid_address_t * a);
  21
  22 u32 mac_parse (u8 * offset, u8 * a);
  23
  24 typedef u16 (*size_to_write_fct) (void *);
  25 typedef void *(*cast_fct) (gid_address_t *);
  26 typedef u16 (*serdes_fct) (u8 *, void *);
  27 typedef u8 (*addr_len_fct) (void *);
  28 typedef void (*copy_fct) (void *, void *);
  29 typedef void (*free_fct) (void *);
  30 typedef int (*cmp_fct) (void *, void *);
  31
  32 size_to_write_fct size_to_write_fcts[GID_ADDR_TYPES] =
  33   { ip_prefix_size_to_write, lcaf_size_to_write, mac_size_to_write,
  34   sd_size_to_write, nsh_size_to_write, 0 /* arp */ , no_addr_size_to_write
  35 };
  36
  37 serdes_fct write_fcts[GID_ADDR_TYPES] =
  38   { ip_prefix_write, lcaf_write, mac_write, sd_write, nsh_write, 0 /* arp */ ,
  39   no_addr_write
  40 };
  41
  42 cast_fct cast_fcts[GID_ADDR_TYPES] =
  43   { ip_prefix_cast, lcaf_cast, mac_cast, sd_cast, nsh_cast, 0 /* arp */ ,
  44   no_addr_cast
  45 };
  46
  47 addr_len_fct addr_len_fcts[GID_ADDR_TYPES] =
  48   { ip_prefix_length, lcaf_length, mac_length, sd_length, nsh_length,
  49   0 /* arp */ , no_addr_length
  50 };
  51
  52 copy_fct copy_fcts[GID_ADDR_TYPES] =
  53   { ip_prefix_copy, lcaf_copy, mac_copy, sd_copy, nsh_copy, 0 /* arp */ ,
  54   no_addr_copy
  55 };
  56
  57 #define foreach_lcaf_type \
  58   _(1, no_addr)      \
  59   _(0, NULL)         \
  60   _(1, vni)          \
  61   _(0, NULL)         \
  62   _(0, NULL)         \
  63   _(0, NULL)         \
  64   _(0, NULL)         \
  65   _(0, NULL)         \
  66   _(0, NULL)         \
  67   _(0, NULL)         \
  68   _(0, NULL)         \
  69   _(0, NULL)         \
  70   _(1, sd)           \
  71   _(0, NULL)         \
  72   _(0, NULL)         \
  73   _(0, NULL)         \
  74   _(0, NULL)         \
  75   _(1, nsh)
  76
  77 #define _(cond, name)                             \
  78   u16 name ## _write (u8 * p, void * a);          \
  79   u16 name ## _parse (u8 * p, void * a);          \
  80   u16 name ## _size_to_write (void * a);          \
  81   void name ## _free (void * a);                  \
  82   void name ## _copy (void * dst, void * src);    \
  83   u8 name ## _length (void * a);                  \
  84   int name ## _cmp (void *, void *);
  85 foreach_lcaf_type
  86 #undef _
  87 #define CONCAT(a,b) a##_##b
  88 #define IF(c, t, e) CONCAT(IF, c)(t, e)
  89 #define IF_0(t, e) e
  90 #define IF_1(t, e) t
  91 #define EXPAND_FCN(cond, fcn)                           \
  92   IF(cond, fcn, NULL)
  93   cmp_fct lcaf_cmp_fcts[LCAF_TYPES] =
  94 {
  95 #define _(cond, name)                                   \
  96     EXPAND_FCN(cond, name##_cmp),
  97   foreach_lcaf_type
  98 #undef _
  99 };
 100
 101 addr_len_fct lcaf_body_length_fcts[LCAF_TYPES] = {
 102 #define _(cond, name)                                   \
 103     EXPAND_FCN(cond, name##_length),
 104   foreach_lcaf_type
 105 #undef _
 106 };
 107
 108 copy_fct lcaf_copy_fcts[LCAF_TYPES] = {
 109 #define _(cond, name)                                   \
 110     EXPAND_FCN(cond, name##_copy),
 111   foreach_lcaf_type
 112 #undef _
 113 };
 114
 115 free_fct lcaf_free_fcts[LCAF_TYPES] = {
 116 #define _(cond, name)                                   \
 117     EXPAND_FCN(cond, name##_free),
 118   foreach_lcaf_type
 119 #undef _
 120 };
 121
 122 size_to_write_fct lcaf_size_to_write_fcts[LCAF_TYPES] = {
 123 #define _(cond, name)                                   \
 124     EXPAND_FCN(cond, name##_size_to_write),
 125   foreach_lcaf_type
 126 #undef _
 127 };
 128
 129 serdes_fct lcaf_write_fcts[LCAF_TYPES] = {
 130 #define _(cond, name)                                   \
 131     EXPAND_FCN(cond, name##_write),
 132   foreach_lcaf_type
 133 #undef _
 134 };
 135
 136 serdes_fct lcaf_parse_fcts[LCAF_TYPES] = {
 137 #define _(cond, name)                                   \
 138     EXPAND_FCN(cond, name##_parse),
 139   foreach_lcaf_type
 140 #undef _
 141 };
 142
 143 u8 *
 144 format_ip_address (u8 * s, va_list * args)
 145 {
 146   ip_address_t *a = va_arg (*args, ip_address_t *);
 147   u8 ver = ip_addr_version (a);
 148   if (ver == IP4)
 149     {
 150       return format (s, "%U", format_ip4_address, &ip_addr_v4 (a));
 151     }
 152   else if (ver == IP6)
 153     {
 154       return format (s, "%U", format_ip6_address, &ip_addr_v6 (a));
 155     }
 156   else
 157     {
 158       clib_warning ("Can't format IP version %d!", ver);
 159       return 0;
 160     }
 161 }
 162
 163 uword
 164 unformat_ip_address (unformat_input_t * input, va_list * args)
 165 {
 166   ip_address_t *a = va_arg (*args, ip_address_t *);
 167
 168   memset (a, 0, sizeof (*a));
 169   if (unformat (input, "%U", unformat_ip4_address, &ip_addr_v4 (a)))
 170     ip_addr_version (a) = IP4;
 171   else if (unformat_user (input, unformat_ip6_address, &ip_addr_v6 (a)))
 172     ip_addr_version (a) = IP6;
 173   else
 174     return 0;
 175   return 1;
 176 }
 177
 178 u8 *
 179 format_ip_prefix (u8 * s, va_list * args)
 180 {
 181   ip_prefix_t *a = va_arg (*args, ip_prefix_t *);
 182   return format (s, "%U/%d", format_ip_address, &ip_prefix_addr (a),
 183                  ip_prefix_len (a));
 184 }
 185
 186 uword
 187 unformat_ip_prefix (unformat_input_t * input, va_list * args)
 188 {
 189   ip_prefix_t *a = va_arg (*args, ip_prefix_t *);
 190   if (unformat (input, "%U/%d", unformat_ip_address, &ip_prefix_addr (a),
 191                 &ip_prefix_len (a)))
 192     {
 193       if ((ip_prefix_version (a) == IP4 && 32 < ip_prefix_len (a)) ||
 194           (ip_prefix_version (a) == IP6 && 128 < ip_prefix_length (a)))
 195         {
 196           clib_warning ("Prefix length to big: %d!", ip_prefix_len (a));
 197           return 0;
 198         }
 199       ip_prefix_normalize (a);
 200     }
 201   else
 202     return 0;
 203   return 1;
 204 }
 205
 206 uword
 207 unformat_nsh_address (unformat_input_t * input, va_list * args)
 208 {
 209   nsh_t *a = va_arg (*args, nsh_t *);
 210   return unformat (input, "SPI:%d SI:%d", &a->spi, &a->si);
 211 }
 212
 213 u8 *
 214 format_nsh_address (u8 * s, va_list * args)
 215 {
 216   nsh_t *a = va_arg (*args, nsh_t *);
 217   return format (s, "SPI:%d SI:%d", a->spi, a->si);
 218 }
 219
 220 u8 *
 221 format_fid_nsh_address (u8 * s, va_list * args)
 222 {
 223   u32 *a = va_arg (*args, u32 *);
 224   return format (s, "SPI:%d SI:%d", *a >> 8, *a & 0xff);
 225 }
 226
 227 u8 *
 228 format_fid_address (u8 * s, va_list * args)
 229 {
 230   fid_address_t *a = va_arg (*args, fid_address_t *);
 231
 232   switch (fid_addr_type (a))
 233     {
 234     case FID_ADDR_IP_PREF:
 235       return format (s, "%U", format_ip_prefix, &fid_addr_ippref (a));
 236     case FID_ADDR_MAC:
 237       return format (s, "%U", format_mac_address, &fid_addr_mac (a));
 238     case FID_ADDR_NSH:
 239       return format (s, "%U", format_fid_nsh_address, &fid_addr_nsh (a));
 240
 241     default:
 242       clib_warning ("Can't format fid address type %d!", fid_addr_type (a));
 243       return 0;
 244     }
 245   return 0;
 246 }
 247
 248 u8 *
 249 format_gid_address (u8 * s, va_list * args)
 250 {
 251   gid_address_t *a = va_arg (*args, gid_address_t *);
 252   u8 type = gid_address_type (a);
 253   switch (type)
 254     {
 255     case GID_ADDR_IP_PREFIX:
 256       return format (s, "[%d] %U", gid_address_vni (a), format_ip_prefix,
 257                      &gid_address_ippref (a));
 258     case GID_ADDR_SRC_DST:
 259       return format (s, "[%d] %U|%U", gid_address_vni (a),
 260                      format_fid_address, &gid_address_sd_src (a),
 261                      format_fid_address, &gid_address_sd_dst (a));
 262     case GID_ADDR_MAC:
 263       return format (s, "[%d] %U", gid_address_vni (a), format_mac_address,
 264                      &gid_address_mac (a));
 265     case GID_ADDR_ARP:
 266     case GID_ADDR_NDP:
 267       return format (s, "[%d, %U]", gid_address_arp_ndp_bd (a),
 268                      format_ip_address, &gid_address_arp_ndp_ip (a));
 269     case GID_ADDR_NSH:
 270       return format (s, "%U", format_nsh_address, &gid_address_nsh (a));
 271
 272     default:
 273       clib_warning ("Can't format gid type %d", type);
 274       return 0;
 275     }
 276   return 0;
 277 }
 278
 279 uword
 280 unformat_fid_address (unformat_input_t * i, va_list * args)
 281 {
 282   fid_address_t *a = va_arg (*args, fid_address_t *);
 283   ip_prefix_t ippref;
 284   u8 mac[6] = { 0 };
 285   nsh_t nsh;
 286
 287   if (unformat (i, "%U", unformat_ip_prefix, &ippref))
 288     {
 289       fid_addr_type (a) = FID_ADDR_IP_PREF;
 290       ip_prefix_copy (&fid_addr_ippref (a), &ippref);
 291     }
 292   else if (unformat (i, "%U", unformat_mac_address, mac))
 293     {
 294       fid_addr_type (a) = FID_ADDR_MAC;
 295       mac_copy (fid_addr_mac (a), mac);
 296     }
 297   else if (unformat (i, "%U", unformat_nsh_address, &nsh))
 298     {
 299       fid_addr_type (a) = FID_ADDR_NSH;
 300       nsh_copy (&fid_addr_nsh (a), &nsh);
 301     }
 302   else
 303     return 0;
 304
 305   return 1;
 306 }
 307
 308 uword
 309 unformat_hmac_key_id (unformat_input_t * input, va_list * args)
 310 {
 311   u32 *key_id = va_arg (*args, u32 *);
 312   u8 *s = 0;
 313
 314   if (unformat (input, "%s", &s))
 315     {
 316       if (!strcmp ((char *) s, "sha1"))
 317         key_id[0] = HMAC_SHA_1_96;
 318       else if (!strcmp ((char *) s, "sha256"))
 319         key_id[0] = HMAC_SHA_256_128;
 320       else
 321         {
 322           clib_warning ("invalid key_id: '%s'", s);
 323           key_id[0] = HMAC_NO_KEY;
 324         }
 325     }
 326   else
 327     return 0;
 328
 329   vec_free (s);
 330   return 1;
 331 }
 332
 333 uword
 334 unformat_gid_address (unformat_input_t * input, va_list * args)
 335 {
 336   gid_address_t *a = va_arg (*args, gid_address_t *);
 337   u8 mac[6] = { 0 };
 338   ip_prefix_t ippref;
 339   fid_address_t sim1, sim2;
 340   nsh_t nsh;
 341
 342   memset (&ippref, 0, sizeof (ippref));
 343   memset (&sim1, 0, sizeof (sim1));
 344   memset (&sim2, 0, sizeof (sim2));
 345
 346   if (unformat (input, "%U|%U", unformat_fid_address, &sim1,
 347                 unformat_fid_address, &sim2))
 348     {
 349       gid_address_sd_src (a) = sim1;
 350       gid_address_sd_dst (a) = sim2;
 351       gid_address_type (a) = GID_ADDR_SRC_DST;
 352     }
 353   else if (unformat (input, "%U", unformat_ip_prefix, &ippref))
 354     {
 355       ip_prefix_copy (&gid_address_ippref (a), &ippref);
 356       gid_address_type (a) = GID_ADDR_IP_PREFIX;
 357     }
 358   else if (unformat (input, "%U", unformat_mac_address, mac))
 359     {
 360       mac_copy (gid_address_mac (a), mac);
 361       gid_address_type (a) = GID_ADDR_MAC;
 362     }
 363   else if (unformat (input, "%U", unformat_nsh_address, &nsh))
 364     {
 365       nsh_copy (&gid_address_nsh (a), &nsh);
 366       gid_address_type (a) = GID_ADDR_NSH;
 367     }
 368   else
 369     return 0;
 370
 371   return 1;
 372 }
 373
 374 uword
 375 unformat_negative_mapping_action (unformat_input_t * input, va_list * args)
 376 {
 377   u32 *action = va_arg (*args, u32 *);
 378   u8 *s = 0;
 379
 380   if (unformat (input, "%s", &s))
 381     {
 382       if (!strcmp ((char *) s, "no-action"))
 383         action[0] = LISP_NO_ACTION;
 384       else if (!strcmp ((char *) s, "natively-forward"))
 385         action[0] = LISP_FORWARD_NATIVE;
 386       else if (!strcmp ((char *) s, "send-map-request"))
 387         action[0] = LISP_SEND_MAP_REQUEST;
 388       else if (!strcmp ((char *) s, "drop"))
 389         action[0] = LISP_DROP;
 390       else
 391         {
 392           clib_warning ("invalid action: '%s'", s);
 393           action[0] = LISP_DROP;
 394         }
 395     }
 396   else
 397     return 0;
 398
 399   vec_free (s);
 400   return 1;
 401 }
 402
 403 u8 *
 404 format_hmac_key_id (u8 * s, va_list * args)
 405 {
 406   lisp_key_type_t key_id = va_arg (*args, lisp_key_type_t);
 407
 408   switch (key_id)
 409     {
 410     case HMAC_SHA_1_96:
 411       return format (0, "sha1");
 412     case HMAC_SHA_256_128:
 413       return format (0, "sha256");
 414     default:
 415       return 0;
 416     }
 417
 418   return 0;
 419 }
 420
 421 u8 *
 422 format_negative_mapping_action (u8 * s, va_list * args)
 423 {
 424   lisp_action_e action = va_arg (*args, lisp_action_e);
 425
 426   switch (action)
 427     {
 428     case LISP_NO_ACTION:
 429       s = format (s, "no-action");
 430       break;
 431     case LISP_FORWARD_NATIVE:
 432       s = format (s, "natively-forward");
 433       break;
 434     case LISP_SEND_MAP_REQUEST:
 435       s = format (s, "send-map-request");
 436       break;
 437     case LISP_DROP:
 438     default:
 439       s = format (s, "drop");
 440       break;
 441     }
 442   return (s);
 443 }
 444
 445 u16
 446 ip_address_size (const ip_address_t * a)
 447 {
 448   switch (ip_addr_version (a))
 449     {
 450     case IP4:
 451       return sizeof (ip4_address_t);
 452       break;
 453     case IP6:
 454       return sizeof (ip6_address_t);
 455       break;
 456     }
 457   return 0;
 458 }
 459
 460 u16
 461 ip_version_to_size (u8 ver)
 462 {
 463   switch (ver)
 464     {
 465     case IP4:
 466       return sizeof (ip4_address_t);
 467       break;
 468     case IP6:
 469       return sizeof (ip6_address_t);
 470       break;
 471     }
 472   return 0;
 473 }
 474
 475 u8
 476 ip_version_to_max_plen (u8 ver)
 477 {
 478   switch (ver)
 479     {
 480     case IP4:
 481       return 32;
 482       break;
 483     case IP6:
 484       return 128;
 485       break;
 486     }
 487   return 0;
 488 }
 489
 490 always_inline lisp_afi_e
 491 ip_version_to_iana_afi (u16 version)
 492 {
 493   switch (version)
 494     {
 495     case IP4:
 496       return LISP_AFI_IP;
 497     case IP6:
 498       return LISP_AFI_IP6;
 499     default:
 500       return 0;
 501     }
 502   return 0;
 503 }
 504
 505 always_inline u8
 506 ip_iana_afi_to_version (lisp_afi_e afi)
 507 {
 508   switch (afi)
 509     {
 510     case LISP_AFI_IP:
 511       return IP4;
 512     case LISP_AFI_IP6:
 513       return IP6;
 514     default:
 515       return 0;
 516     }
 517   return 0;
 518 }
 519
 520 u16
 521 ip_address_size_to_write (ip_address_t * a)
 522 {
 523   return ip_address_size (a) + sizeof (u16);
 524 }
 525
 526 u16
 527 ip_address_iana_afi (ip_address_t * a)
 528 {
 529   return ip_version_to_iana_afi (ip_addr_version (a));
 530 }
 531
 532 u8
 533 ip_address_max_len (u8 version)
 534 {
 535   return version == IP4 ? 32 : 128;
 536 }
 537
 538 u16
 539 ip4_address_size_to_put ()
 540 {
 541   // return sizeof(u16) + sizeof (ip4_address_t);
 542   return 6;
 543 }
 544
 545 u16
 546 ip6_address_size_to_put ()
 547 {
 548   //return sizeof(u16) + sizeof (ip6_address_t);
 549   return 18;
 550 }
 551
 552 u32
 553 ip4_address_put (u8 * b, ip4_address_t * a)
 554 {
 555   *(u16 *) b = clib_host_to_net_u16 (ip_version_to_iana_afi (IP4));
 556   u8 *p = b + sizeof (u16);
 557   clib_memcpy (p, a, sizeof (*a));
 558   return ip4_address_size_to_put ();
 559 }
 560
 561 u32
 562 ip6_address_put (u8 * b, ip6_address_t * a)
 563 {
 564   *(u16 *) b = clib_host_to_net_u16 (ip_version_to_iana_afi (IP6));
 565   u8 *p = b + sizeof (u16);
 566   clib_memcpy (p, a, sizeof (*a));
 567   return ip6_address_size_to_put ();
 568 }
 569
 570 u32
 571 ip_address_put (u8 * b, ip_address_t * a)
 572 {
 573   u32 len = ip_address_size (a);
 574   *(u16 *) b = clib_host_to_net_u16 (ip_address_iana_afi (a));
 575   u8 *p = b + sizeof (u16);
 576   clib_memcpy (p, &ip_addr_addr (a), len);
 577   return (len + sizeof (u16));
 578 }
 579
 580 u32
 581 ip_address_parse (void *offset, u16 iana_afi, ip_address_t * dst)
 582 {
 583   ip_addr_version (dst) = ip_iana_afi_to_version (iana_afi);
 584   u8 size = ip_version_to_size (ip_addr_version (dst));
 585   clib_memcpy (&ip_addr_addr (dst), offset + sizeof (u16), size);
 586   return (sizeof (u16) + size);
 587 }
 588
 589 void
 590 gid_to_dp_address (gid_address_t * g, dp_address_t * d)
 591 {
 592   switch (gid_address_type (g))
 593     {
 594     case GID_ADDR_SRC_DST:
 595       switch (gid_address_sd_dst_type (g))
 596         {
 597         case FID_ADDR_IP_PREF:
 598           ip_prefix_copy (&d->ippref, &gid_address_sd_dst_ippref (g));
 599           d->type = FID_ADDR_IP_PREF;
 600           break;
 601         case FID_ADDR_MAC:
 602           mac_copy (&d->mac, &gid_address_sd_dst_mac (g));
 603           d->type = FID_ADDR_MAC;
 604           break;
 605         default:
 606           clib_warning ("Source/Dest address type %d not supported!",
 607                         gid_address_sd_dst_type (g));
 608           break;
 609         }
 610       break;
 611     case GID_ADDR_IP_PREFIX:
 612       ip_prefix_copy (&d->ippref, &gid_address_ippref (g));
 613       d->type = FID_ADDR_IP_PREF;
 614       break;
 615     case GID_ADDR_MAC:
 616       mac_copy (&d->mac, &gid_address_mac (g));
 617       d->type = FID_ADDR_MAC;
 618       break;
 619     case GID_ADDR_NSH:
 620     default:
 621       d->nsh = gid_address_nsh (g).spi << 8 | gid_address_nsh (g).si;
 622       d->type = FID_ADDR_NSH;
 623       break;
 624     }
 625 }
 626
 627 u32
 628 lcaf_hdr_parse (void *offset, lcaf_t * lcaf)
 629 {
 630   lcaf_hdr_t *lh = offset;
 631   lcaf->type = lh->type;
 632
 633   /* this is a bit of hack: since the LCAF Instance ID is the
 634      only message that uses reserved2 field, we can set it here.
 635      If any LCAF format starts using reserved2 field as well this needs
 636      to be moved elsewhere */
 637   lcaf_vni_len (lcaf) = lh->reserved2;
 638
 639   return sizeof (lh[0]);
 640 }
 641
 642 static u8
 643 iana_afi_to_fid_addr_type (u16 type)
 644 {
 645   switch (type)
 646     {
 647     case LISP_AFI_IP:
 648     case LISP_AFI_IP6:
 649       return FID_ADDR_IP_PREF;
 650
 651     case LISP_AFI_MAC:
 652       return FID_ADDR_MAC;
 653     }
 654   return ~0;
 655 }
 656
 657 static u16
 658 fid_addr_parse (u8 * p, fid_address_t * a)
 659 {
 660   u16 afi = clib_net_to_host_u16 (*(u16 *) p);
 661   fid_addr_type (a) = iana_afi_to_fid_addr_type (afi);
 662   ip_address_t *ip_addr = &ip_prefix_addr (&fid_addr_ippref (a));
 663
 664   switch (fid_addr_type (a))
 665     {
 666     case FID_ADDR_MAC:
 667       return mac_parse (p, fid_addr_mac (a));
 668
 669     case FID_ADDR_IP_PREF:
 670       return ip_address_parse (p, afi, ip_addr);
 671
 672     case FID_ADDR_NSH:
 673       break;
 674     }
 675   return ~0;
 676 }
 677
 678 #define INC(dst, exp)   \
 679 do {                    \
 680   u16 _sum = (exp);     \
 681   if ((u16)~0 == _sum)  \
 682     return ~0;          \
 683   dst += _sum;          \
 684 } while (0);
 685
 686 void
 687 nsh_free (void *a)
 688 {
 689   /* nothing to do */
 690 }
 691
 692 u16
 693 nsh_parse (u8 * p, void *a)
 694 {
 695   lcaf_spi_hdr_t *h = (lcaf_spi_hdr_t *) p;
 696   gid_address_t *g = a;
 697
 698   gid_address_type (g) = GID_ADDR_NSH;
 699   gid_address_nsh_spi (g) = clib_net_to_host_u32 (LCAF_SPI_SI (h)) >> 8;
 700   gid_address_nsh_si (g) = (u8) clib_net_to_host_u32 (LCAF_SPI_SI (h));
 701
 702   return sizeof (lcaf_spi_hdr_t);
 703 }
 704
 705 int
 706 nsh_cmp (void *a1, void *a2)
 707 {
 708   nsh_t *n1 = a1;
 709   nsh_t *n2 = a2;
 710
 711   if (n1->spi != n2->spi)
 712     return 1;
 713   if (n1->si != n2->si)
 714     return 1;
 715   return 0;
 716 }
 717
 718 u16
 719 sd_parse (u8 * p, void *a)
 720 {
 721   lcaf_src_dst_hdr_t *sd_hdr;
 722   gid_address_t *g = a;
 723   u16 size = 0;
 724   fid_address_t *src = &gid_address_sd_src (g);
 725   fid_address_t *dst = &gid_address_sd_dst (g);
 726
 727   gid_address_type (g) = GID_ADDR_SRC_DST;
 728
 729   sd_hdr = (lcaf_src_dst_hdr_t *) (p + size);
 730   size += sizeof (sd_hdr[0]);
 731
 732   INC (size, fid_addr_parse (p + size, src));
 733   INC (size, fid_addr_parse (p + size, dst));
 734
 735   if (fid_addr_type (src) == FID_ADDR_IP_PREF)
 736     {
 737       ip_prefix_t *ippref = &fid_addr_ippref (src);
 738       ip_prefix_len (ippref) = LCAF_SD_SRC_ML (sd_hdr);
 739     }
 740   if (fid_addr_type (dst) == FID_ADDR_IP_PREF)
 741     {
 742       ip_prefix_t *ippref = &fid_addr_ippref (dst);
 743       ip_prefix_len (ippref) = LCAF_SD_DST_ML (sd_hdr);
 744     }
 745   return size;
 746 }
 747
 748 u16
 749 try_parse_src_dst_lcaf (u8 * p, gid_address_t * a)
 750 {
 751   lcaf_t lcaf;
 752   u16 size = sizeof (u16);      /* skip AFI */
 753
 754   size += lcaf_hdr_parse (p + size, &lcaf);
 755
 756   if (LCAF_SOURCE_DEST != lcaf_type (&lcaf))
 757     return ~0;
 758
 759   INC (size, sd_parse (p + size, a));
 760   return size;
 761 }
 762
 763 u16
 764 vni_parse (u8 * p, void *a)
 765 {
 766   lcaf_t *lcaf = a;
 767   gid_address_t *g = a;
 768   u16 size = 0;
 769
 770   gid_address_vni (g) = clib_net_to_host_u32 (*(u32 *) p);
 771   size += sizeof (u32);
 772   gid_address_vni_mask (g) = lcaf_vni_len (lcaf);
 773
 774   /* nested LCAFs are not supported except of src/dst with vni - to handle
 775    * such case look at the next AFI and process src/dest LCAF separately */
 776   u16 afi = clib_net_to_host_u16 (*((u16 *) (p + size)));
 777   if (LISP_AFI_LCAF == afi)
 778     {
 779       INC (size, try_parse_src_dst_lcaf (p + size, g));
 780     }
 781   else
 782     INC (size, gid_address_parse (p + size, g));
 783
 784   return size;
 785 }
 786
 787 u16
 788 no_addr_parse (u8 * p, void *a)
 789 {
 790   /* do nothing */
 791   return 0;
 792 }
 793
 794 u32
 795 lcaf_parse (void *offset, gid_address_t * addr)
 796 {
 797   /* skip AFI type */
 798   offset += sizeof (u16);
 799   lcaf_t *lcaf = &gid_address_lcaf (addr);
 800
 801   u32 size = lcaf_hdr_parse (offset, lcaf);
 802   u8 type = lcaf_type (lcaf);
 803
 804   if (!lcaf_parse_fcts[type])
 805     {
 806       clib_warning ("Unsupported LCAF type: %u", type);
 807       return ~0;
 808     }
 809   INC (size, (*lcaf_parse_fcts[type]) (offset + size, lcaf));
 810   return sizeof (u16) + size;
 811 }
 812
 813 void
 814 vni_free (void *a)
 815 {
 816   vni_t *v = a;
 817   gid_address_free (vni_gid (v));
 818   clib_mem_free (vni_gid (v));
 819 }
 820
 821 void
 822 no_addr_free (void *a)
 823 {
 824   /* nothing to do */
 825 }
 826
 827 void
 828 sd_free (void *a)
 829 {
 830   /* nothing */
 831 }
 832
 833 void
 834 gid_address_free (gid_address_t * a)
 835 {
 836   if (gid_address_type (a) != GID_ADDR_LCAF)
 837     return;
 838
 839   lcaf_t *lcaf = &gid_address_lcaf (a);
 840   u8 lcaf_type = lcaf_type (lcaf);
 841   (*lcaf_free_fcts[lcaf_type]) (lcaf);
 842 }
 843
 844 void
 845 gid_address_from_ip (gid_address_t * g, ip_address_t * ip)
 846 {
 847   memset (g, 0, sizeof (g[0]));
 848   ip_address_set (&gid_address_ip (g), ip, ip_addr_version (ip));
 849   gid_address_ippref_len (g) = 32;
 850 }
 851
 852 int
 853 ip_address_cmp (const ip_address_t * ip1, const ip_address_t * ip2)
 854 {
 855   int res = 0;
 856   if (ip_addr_version (ip1) != ip_addr_version (ip2))
 857     return -1;
 858   res =
 859     memcmp (&ip_addr_addr (ip1), &ip_addr_addr (ip2), ip_address_size (ip1));
 860
 861   if (res < 0)
 862     res = 2;
 863   else if (res > 0)
 864     res = 1;
 865
 866   return res;
 867 }
 868
 869 void
 870 ip_address_copy (ip_address_t * dst, const ip_address_t * src)
 871 {
 872   if (IP4 == ip_addr_version (src))
 873     {
 874       /* don't copy any garbage from the union */
 875       memset (dst, 0, sizeof (*dst));
 876       dst->ip.v4 = src->ip.v4;
 877       dst->version = IP4;
 878     }
 879   else
 880     {
 881       clib_memcpy (dst, src, sizeof (ip_address_t));
 882     }
 883 }
 884
 885 void
 886 ip_address_copy_addr (void *dst, const ip_address_t * src)
 887 {
 888   clib_memcpy (dst, src, ip_address_size (src));
 889 }
 890
 891 void
 892 ip_address_set (ip_address_t * dst, const void *src, u8 version)
 893 {
 894   clib_memcpy (dst, src, ip_version_to_size (version));
 895   ip_addr_version (dst) = version;
 896 }
 897
 898 void
 899 ip_address_to_46 (const ip_address_t * addr,
 900                   ip46_address_t * a, fib_protocol_t * proto)
 901 {
 902   *proto = (IP4 == ip_addr_version (addr) ?
 903             FIB_PROTOCOL_IP4 : FIB_PROTOCOL_IP6);
 904   switch (*proto)
 905     {
 906     case FIB_PROTOCOL_IP4:
 907       ip46_address_set_ip4 (a, &addr->ip.v4);
 908       break;
 909     case FIB_PROTOCOL_IP6:
 910       a->ip6 = addr->ip.v6;
 911       break;
 912     default:
 913       ASSERT (0);
 914       break;
 915     }
 916 }
 917
 918 static void
 919 ip_prefix_normalize_ip4 (ip4_address_t * ip4, u8 preflen)
 920 {
 921   u32 mask = ~0;
 922
 923   ASSERT (ip4);
 924
 925   if (32 <= preflen)
 926     {
 927       return;
 928     }
 929
 930   mask = pow2_mask (preflen) << (32 - preflen);
 931   mask = clib_host_to_net_u32 (mask);
 932   ip4->data_u32 &= mask;
 933 }
 934
 935 static void
 936 ip_prefix_normalize_ip6 (ip6_address_t * ip6, u8 preflen)
 937 {
 938   u8 mask_6[16];
 939   u32 *m;
 940   u8 j, i0, i1;
 941
 942   ASSERT (ip6);
 943
 944   memset (mask_6, 0, sizeof (mask_6));
 945
 946   if (128 <= preflen)
 947     {
 948       return;
 949     }
 950
 951   i1 = preflen % 32;
 952   i0 = preflen / 32;
 953   m = (u32 *) & mask_6[0];
 954
 955   for (j = 0; j < i0; j++)
 956     {
 957       m[j] = ~0;
 958     }
 959
 960   if (i1)
 961     {
 962       m[i0] = clib_host_to_net_u32 (pow2_mask (i1) << (32 - i1));
 963     }
 964
 965   for (j = 0; j < sizeof (mask_6); j++)
 966     {
 967       ip6->as_u8[j] &= mask_6[j];
 968     }
 969 }
 970
 971 void
 972 ip_prefix_normalize (ip_prefix_t * a)
 973 {
 974   u8 preflen = ip_prefix_len (a);
 975
 976   switch (ip_prefix_version (a))
 977     {
 978     case IP4:
 979       ip_prefix_normalize_ip4 (&ip_prefix_v4 (a), preflen);
 980       break;
 981
 982     case IP6:
 983       ip_prefix_normalize_ip6 (&ip_prefix_v6 (a), preflen);
 984       break;
 985
 986     default:
 987       ASSERT (0);
 988     }
 989 }
 990
 991 void *
 992 ip_prefix_cast (gid_address_t * a)
 993 {
 994   return &gid_address_ippref (a);
 995 }
 996
 997 u16
 998 ip_prefix_size_to_write (void *pref)
 999 {
1000   ip_prefix_t *a = (ip_prefix_t *) pref;
1001   return ip_address_size_to_write (&ip_prefix_addr (a));
1002 }
1003
1004 u16
1005 ip_prefix_write (u8 * p, void *gid)
1006 {
1007   gid_address_t *g = gid;
1008   ip_prefix_t *a = &gid_address_ippref (g);
1009
1010   switch (ip_prefix_version (a))
1011     {
1012     case IP4:
1013       return ip4_address_put (p, &ip_prefix_v4 (a));
1014       break;
1015     case IP6:
1016       return ip6_address_put (p, &ip_prefix_v6 (a));
1017       break;
1018     }
1019   return 0;
1020 }
1021
1022 u8
1023 ip_prefix_length (void *a)
1024 {
1025   return ip_prefix_len ((ip_prefix_t *) a);
1026 }
1027
1028 void
1029 ip_prefix_copy (void *dst, void *src)
1030 {
1031   clib_memcpy (dst, src, sizeof (ip_prefix_t));
1032 }
1033
1034 void
1035 mac_copy (void *dst, void *src)
1036 {
1037   clib_memcpy (dst, src, 6);
1038 }
1039
1040 void
1041 sd_copy (void *dst, void *src)
1042 {
1043   clib_memcpy (dst, src, sizeof (source_dest_t));
1044 }
1045
1046 void
1047 nsh_copy (void *dst, void *src)
1048 {
1049   clib_memcpy (dst, src, sizeof (nsh_t));
1050 }
1051
1052 int
1053 ip_prefix_cmp (ip_prefix_t * p1, ip_prefix_t * p2)
1054 {
1055   int cmp = 0;
1056
1057   ip_prefix_normalize (p1);
1058   ip_prefix_normalize (p2);
1059
1060   cmp = ip_address_cmp (&ip_prefix_addr (p1), &ip_prefix_addr (p2));
1061   if (cmp == 0)
1062     {
1063       if (ip_prefix_len (p1) < ip_prefix_len (p2))
1064         {
1065           cmp = 1;
1066         }
1067       else
1068         {
1069           if (ip_prefix_len (p1) > ip_prefix_len (p2))
1070             cmp = 2;
1071         }
1072     }
1073   return cmp;
1074 }
1075
1076 void
1077 no_addr_copy (void *dst, void *src)
1078 {
1079   /* nothing to do */
1080 }
1081
1082 void
1083 vni_copy (void *dst, void *src)
1084 {
1085   vni_t *vd = dst;
1086   vni_t *vs = src;
1087
1088   clib_memcpy (vd, vs, sizeof (vd[0]));
1089   vni_gid (vd) = clib_mem_alloc (sizeof (gid_address_t));
1090   gid_address_copy (vni_gid (vd), vni_gid (vs));
1091 }
1092
1093 void
1094 lcaf_copy (void *dst, void *src)
1095 {
1096   lcaf_t *lcaf_dst = dst;
1097   lcaf_t *lcaf_src = src;
1098
1099   lcaf_type (lcaf_dst) = lcaf_type (lcaf_src);
1100   (*lcaf_copy_fcts[lcaf_type (lcaf_src)]) (dst, src);
1101 }
1102
1103 u8
1104 lcaf_length (void *a)
1105 {
1106   return 0;
1107 }
1108
1109 u8
1110 mac_length (void *a)
1111 {
1112   return 0;
1113 }
1114
1115 u8
1116 sd_length (void *a)
1117 {
1118   return 0;
1119 }
1120
1121 u8
1122 nsh_length (void *a)
1123 {
1124   return 0;
1125 }
1126
1127 void *
1128 lcaf_cast (gid_address_t * a)
1129 {
1130   return &gid_address_lcaf (a);
1131 }
1132
1133 void *
1134 mac_cast (gid_address_t * a)
1135 {
1136   return &gid_address_mac (a);
1137 }
1138
1139 void *
1140 no_addr_cast (gid_address_t * a)
1141 {
1142   return (void *) a;
1143 }
1144
1145 void *
1146 sd_cast (gid_address_t * a)
1147 {
1148   return &gid_address_sd (a);
1149 }
1150
1151 void *
1152 nsh_cast (gid_address_t * a)
1153 {
1154   return &gid_address_nsh (a);
1155 }
1156
1157 u8
1158 no_addr_length (void *a)
1159 {
1160   return 0;
1161 }
1162
1163 u8
1164 vni_length (void *a)
1165 {
1166   vni_t *v = a;
1167   return (sizeof (u32)          /* VNI size */
1168           + gid_address_size_to_put (vni_gid (v)) /* vni body size */ );
1169 }
1170
1171 u16
1172 lcaf_write (u8 * p, void *a)
1173 {
1174   u16 size = 0, len;
1175   lcaf_t *lcaf = a;
1176   u8 type = lcaf_type (lcaf);
1177   lcaf_hdr_t _h, *h = &_h;
1178
1179   *(u16 *) p = clib_host_to_net_u16 (LISP_AFI_LCAF);
1180   size += sizeof (u16);
1181   memset (h, 0, sizeof (h[0]));
1182   LCAF_TYPE (h) = type;
1183   u16 lcaf_len = (*lcaf_body_length_fcts[type]) (lcaf);
1184   LCAF_LENGTH (h) = clib_host_to_net_u16 (lcaf_len);
1185
1186   clib_memcpy (p + size, h, sizeof (h[0]));
1187   size += sizeof (h[0]);
1188   len = (*lcaf_write_fcts[type]) (p + size, lcaf);
1189
1190   if ((u16) ~ 0 == len)
1191     return ~0;
1192
1193   return size + len;
1194 }
1195
1196 u16
1197 mac_write (u8 * p, void *a)
1198 {
1199   *(u16 *) p = clib_host_to_net_u16 (LISP_AFI_MAC);
1200   clib_memcpy (p + sizeof (u16), a, 6);
1201   return mac_size_to_write (a);
1202 }
1203
1204 static u16
1205 fid_addr_write (u8 * p, fid_address_t * a)
1206 {
1207   switch (fid_addr_type (a))
1208     {
1209     case FID_ADDR_IP_PREF:
1210       return ip_prefix_write (p, &fid_addr_ippref (a));
1211
1212     case FID_ADDR_MAC:
1213       return mac_write (p, &fid_addr_mac (a));
1214
1215     default:
1216       return ~0;
1217     }
1218   return ~0;
1219 }
1220
1221 static u8
1222 fid_address_length (fid_address_t * a)
1223 {
1224   switch (fid_addr_type (a))
1225     {
1226     case FID_ADDR_IP_PREF:
1227       return ip_prefix_length (&fid_addr_ippref (a));
1228     case FID_ADDR_MAC:
1229       return 0;
1230     case FID_ADDR_NSH:
1231       return 0;
1232     }
1233   return 0;
1234 }
1235
1236 u16
1237 sd_write (u8 * p, void *a)
1238 {
1239   source_dest_t *sd = a;
1240   u16 size = 0;
1241   lcaf_hdr_t _h, *h = &_h;
1242   lcaf_src_dst_hdr_t sd_hdr;
1243
1244   *(u16 *) p = clib_host_to_net_u16 (LISP_AFI_LCAF);
1245   size += sizeof (u16);
1246   memset (h, 0, sizeof (h[0]));
1247   LCAF_TYPE (h) = LCAF_SOURCE_DEST;
1248   u16 lcaf_len = sizeof (lcaf_src_dst_hdr_t)
1249     + fid_addr_size_to_write (&sd_src (sd))
1250     + fid_addr_size_to_write (&sd_dst (sd));
1251   LCAF_LENGTH (h) = clib_host_to_net_u16 (lcaf_len);
1252
1253   clib_memcpy (p + size, h, sizeof (h[0]));
1254   size += sizeof (h[0]);
1255
1256   memset (&sd_hdr, 0, sizeof (sd_hdr));
1257   LCAF_SD_SRC_ML (&sd_hdr) = fid_address_length (&sd_src (sd));
1258   LCAF_SD_DST_ML (&sd_hdr) = fid_address_length (&sd_dst (sd));
1259   clib_memcpy (p + size, &sd_hdr, sizeof (sd_hdr));
1260   size += sizeof (sd_hdr);
1261
1262   u16 len = fid_addr_write (p + size, &sd_src (sd));
1263   if ((u16) ~ 0 == len)
1264     return ~0;
1265   size += len;
1266
1267   len = fid_addr_write (p + size, &sd_dst (sd));
1268   if ((u16) ~ 0 == len)
1269     return ~0;
1270   size += len;
1271
1272   return size;
1273 }
1274
1275 u16
1276 nsh_write (u8 * p, void *a)
1277 {
1278   lcaf_spi_hdr_t spi;
1279   lcaf_hdr_t lcaf;
1280   gid_address_t *g = a;
1281   u16 size = 0;
1282
1283   ASSERT (gid_address_type (g) == GID_ADDR_NSH);
1284
1285   memset (&lcaf, 0, sizeof (lcaf));
1286   memset (&spi, 0, sizeof (spi));
1287
1288   LCAF_TYPE (&lcaf) = LCAF_NSH;
1289   LCAF_LENGTH (&lcaf) = clib_host_to_net_u16 (sizeof (lcaf_spi_hdr_t));
1290
1291   u32 s = clib_host_to_net_u32 (gid_address_nsh_spi (g) << 8 |
1292                                 gid_address_nsh_si (g));
1293   LCAF_SPI_SI (&spi) = s;
1294
1295   *(u16 *) p = clib_host_to_net_u16 (LISP_AFI_LCAF);
1296   size += sizeof (u16);
1297
1298   clib_memcpy (p + size, &lcaf, sizeof (lcaf));
1299   size += sizeof (lcaf);
1300
1301   clib_memcpy (p + size, &spi, sizeof (spi));
1302   size += sizeof (spi);
1303
1304   return size;
1305 }
1306
1307 u16
1308 vni_write (u8 * p, void *a)
1309 {
1310   lcaf_hdr_t _h, *h = &_h;
1311   gid_address_t *g = a;
1312   u16 size = 0, len;
1313
1314   /* put lcaf header */
1315   *(u16 *) p = clib_host_to_net_u16 (LISP_AFI_LCAF);
1316   size += sizeof (u16);
1317   memset (h, 0, sizeof (h[0]));
1318   LCAF_TYPE (h) = LCAF_INSTANCE_ID;
1319   u16 lcaf_len = sizeof (u32)   /* Instance ID size */
1320     + gid_address_size_to_put_no_vni (g);
1321   LCAF_LENGTH (h) = clib_host_to_net_u16 (lcaf_len);
1322   LCAF_RES2 (h) = gid_address_vni_mask (g);
1323
1324   /* put vni header */
1325   clib_memcpy (p + size, h, sizeof (h[0]));
1326   size += sizeof (h[0]);
1327
1328   u32 *afip = (u32 *) (p + size);
1329   afip[0] = clib_host_to_net_u32 (gid_address_vni (g));
1330   size += sizeof (u32);
1331
1332   if (GID_ADDR_SRC_DST == gid_address_type (g))
1333     /* write src/dst LCAF */
1334     {
1335       len = sd_write (p + size, g);
1336       if ((u16) ~ 0 == len)
1337         return ~0;
1338     }
1339   else
1340     /* write the actual address */
1341     len = gid_address_put_no_vni (p + size, g);
1342
1343   if ((u16) ~ 0 == len)
1344     return ~0;
1345
1346   return size + len;
1347 }
1348
1349 u16
1350 no_addr_write (u8 * p, void *a)
1351 {
1352   /* do nothing; return AFI field size */
1353   return sizeof (u16);
1354 }
1355
1356 u16
1357 no_addr_size_to_write (void *a)
1358 {
1359   return sizeof (u16);          /* AFI field length */
1360 }
1361
1362 static u16
1363 fid_addr_size_to_write (fid_address_t * a)
1364 {
1365   switch (fid_addr_type (a))
1366     {
1367     case FID_ADDR_IP_PREF:
1368       return ip_prefix_size_to_write (a);
1369
1370     case FID_ADDR_MAC:
1371       return mac_size_to_write (a);
1372
1373     default:
1374       break;
1375     }
1376   return 0;
1377 }
1378
1379 u16
1380 vni_size_to_write (void *a)
1381 {
1382   gid_address_t *g = a;
1383
1384   u16 lcaf_size = sizeof (u32) + sizeof (u16)   /* LCAF AFI field size */
1385     + sizeof (lcaf_hdr_t);
1386
1387   if (gid_address_type (g) == GID_ADDR_SRC_DST)
1388     /* special case where nested LCAF is supported */
1389     return lcaf_size + sd_size_to_write (g);
1390   else
1391     return lcaf_size + gid_address_size_to_put_no_vni (g);
1392 }
1393
1394 u16
1395 lcaf_size_to_write (void *a)
1396 {
1397   lcaf_t *lcaf = (lcaf_t *) a;
1398   u32 size = 0, len;
1399   u8 type = lcaf_type (lcaf);
1400
1401   size += sizeof (u16);         /* AFI size */
1402
1403   len = (*lcaf_size_to_write_fcts[type]) (lcaf);
1404   if (~0 == len)
1405     return ~0;
1406
1407   return size + len;
1408 }
1409
1410 u16
1411 sd_size_to_write (void *a)
1412 {
1413   source_dest_t *sd = a;
1414   return sizeof (u16)
1415     + sizeof (lcaf_hdr_t)
1416     + sizeof (lcaf_src_dst_hdr_t)
1417     + fid_addr_size_to_write (&sd_src (sd))
1418     + fid_addr_size_to_write (&sd_dst (sd));
1419 }
1420
1421 u16
1422 mac_size_to_write (void *a)
1423 {
1424   return sizeof (u16) + 6;
1425 }
1426
1427 u16
1428 nsh_size_to_write (void *a)
1429 {
1430   return sizeof (u16) + sizeof (lcaf_hdr_t) + sizeof (lcaf_spi_hdr_t);
1431 }
1432
1433 u8
1434 gid_address_len (gid_address_t * a)
1435 {
1436   gid_address_type_t type = gid_address_type (a);
1437   return (*addr_len_fcts[type]) ((*cast_fcts[type]) (a));
1438 }
1439
1440 static u16
1441 gid_address_put_no_vni (u8 * b, gid_address_t * gid)
1442 {
1443   gid_address_type_t type = gid_address_type (gid);
1444   return (*write_fcts[type]) (b, (*cast_fcts[type]) (gid));
1445 }
1446
1447 u16
1448 gid_address_put (u8 * b, gid_address_t * gid)
1449 {
1450   if (0 != gid_address_vni (gid))
1451     return vni_write (b, gid);
1452
1453   return gid_address_put_no_vni (b, gid);
1454 }
1455
1456 static u16
1457 gid_address_size_to_put_no_vni (gid_address_t * gid)
1458 {
1459   gid_address_type_t type = gid_address_type (gid);
1460   return (*size_to_write_fcts[type]) ((*cast_fcts[type]) (gid));
1461 }
1462
1463 u16
1464 gid_address_size_to_put (gid_address_t * gid)
1465 {
1466   if (0 != gid_address_vni (gid))
1467     return vni_size_to_write (gid);
1468
1469   return gid_address_size_to_put_no_vni (gid);
1470 }
1471
1472 void *
1473 gid_address_cast (gid_address_t * gid, gid_address_type_t type)
1474 {
1475   return (*cast_fcts[type]) (gid);
1476 }
1477
1478 void
1479 gid_address_copy (gid_address_t * dst, gid_address_t * src)
1480 {
1481   gid_address_type_t type = gid_address_type (src);
1482   (*copy_fcts[type]) ((*cast_fcts[type]) (dst), (*cast_fcts[type]) (src));
1483   gid_address_type (dst) = type;
1484   gid_address_vni (dst) = gid_address_vni (src);
1485   gid_address_vni_mask (dst) = gid_address_vni_mask (src);
1486 }
1487
1488 u32
1489 mac_parse (u8 * offset, u8 * a)
1490 {
1491   /* skip AFI field */
1492   offset += sizeof (u16);
1493
1494   clib_memcpy (a, offset, 6);
1495   return sizeof (u16) + 6;
1496 }
1497
1498 u32
1499 gid_address_parse (u8 * offset, gid_address_t * a)
1500 {
1501   lisp_afi_e afi;
1502   u16 len = 0;
1503
1504   ASSERT (a);
1505
1506   /* NOTE: since gid_address_parse may be called by vni_parse, we can't 0
1507    * the gid address here */
1508   afi = clib_net_to_host_u16 (*((u16 *) offset));
1509
1510   switch (afi)
1511     {
1512     case LISP_AFI_NO_ADDR:
1513       len = sizeof (u16);
1514       gid_address_type (a) = GID_ADDR_NO_ADDRESS;
1515       break;
1516     case LISP_AFI_IP:
1517       len = ip_address_parse (offset, afi, &gid_address_ip (a));
1518       gid_address_type (a) = GID_ADDR_IP_PREFIX;
1519       /* this should be modified outside if needed */
1520       gid_address_ippref_len (a) = 32;
1521       break;
1522     case LISP_AFI_IP6:
1523       len = ip_address_parse (offset, afi, &gid_address_ip (a));
1524       gid_address_type (a) = GID_ADDR_IP_PREFIX;
1525       /* this should be modified outside if needed */
1526       gid_address_ippref_len (a) = 128;
1527       break;
1528     case LISP_AFI_LCAF:
1529       gid_address_type (a) = GID_ADDR_LCAF;
1530       len = lcaf_parse (offset, a);
1531       break;
1532     case LISP_AFI_MAC:
1533       len = mac_parse (offset, gid_address_mac (a));
1534       gid_address_type (a) = GID_ADDR_MAC;
1535       break;
1536     default:
1537       clib_warning ("LISP AFI %d not supported!", afi);
1538       return ~0;
1539     }
1540   return (len == (u16) ~ 0) ? ~0 : len;
1541 }
1542
1543 void
1544 gid_address_ip_set (gid_address_t * dst, void *src, u8 version)
1545 {
1546   gid_address_ippref_len (dst) = ip_address_max_len (version);
1547   ip_address_set (&gid_address_ip (dst), src, version);
1548 }
1549
1550 int
1551 no_addr_cmp (void *a1, void *a2)
1552 {
1553   return 0;
1554 }
1555
1556 int
1557 vni_cmp (void *a1, void *a2)
1558 {
1559   vni_t *v1 = a1;
1560   vni_t *v2 = a2;
1561
1562   if (vni_mask_len (v1) != vni_mask_len (v2))
1563     return -1;
1564   if (vni_vni (v1) != vni_vni (v2))
1565     return -1;
1566   return gid_address_cmp (vni_gid (v1), vni_gid (v2));
1567 }
1568
1569 static int
1570 mac_cmp (void *a1, void *a2)
1571 {
1572   return memcmp (a1, a2, 6);
1573 }
1574
1575 static int
1576 fid_addr_cmp (fid_address_t * a1, fid_address_t * a2)
1577 {
1578   if (fid_addr_type (a1) != fid_addr_type (a2))
1579     return -1;
1580
1581   switch (fid_addr_type (a1))
1582     {
1583     case FID_ADDR_IP_PREF:
1584       return ip_prefix_cmp (&fid_addr_ippref (a1), &fid_addr_ippref (a2));
1585
1586     case FID_ADDR_MAC:
1587       return mac_cmp (fid_addr_mac (a1), fid_addr_mac (a2));
1588
1589     default:
1590       return -1;
1591     }
1592   return -1;
1593 }
1594
1595 int
1596 sd_cmp (void *a1, void *a2)
1597 {
1598   source_dest_t *sd1 = a1;
1599   source_dest_t *sd2 = a2;
1600
1601   if (fid_addr_cmp (&sd_dst (sd1), &sd_dst (sd2)))
1602     return -1;
1603   if (fid_addr_cmp (&sd_src (sd1), &sd_src (sd2)))
1604     return -1;
1605   return 0;
1606 }
1607
1608 /* Compare two gid_address_t.
1609  * Returns:
1610  *        -1: If they are from different afi
1611  *             0: Both address are the same
1612  *             1: Addr1 is bigger than addr2
1613  *             2: Addr2 is bigger than addr1
1614  */
1615 int
1616 gid_address_cmp (gid_address_t * a1, gid_address_t * a2)
1617 {
1618   lcaf_t *lcaf1, *lcaf2;
1619   int cmp = -1;
1620   if (!a1 || !a2)
1621     return -1;
1622   if (gid_address_type (a1) != gid_address_type (a2))
1623     return -1;
1624   if (gid_address_vni (a1) != gid_address_vni (a2))
1625     return -1;
1626
1627   /* TODO vni mask is not supported, disable comparing for now
1628      if (gid_address_vni_mask (a1) != gid_address_vni_mask (a2))
1629      return -1;
1630    */
1631
1632   switch (gid_address_type (a1))
1633     {
1634     case GID_ADDR_NO_ADDRESS:
1635       if (a1 == a2)
1636         cmp = 0;
1637       else
1638         cmp = 2;
1639       break;
1640     case GID_ADDR_IP_PREFIX:
1641       cmp =
1642         ip_prefix_cmp (&gid_address_ippref (a1), &gid_address_ippref (a2));
1643       break;
1644     case GID_ADDR_LCAF:
1645       lcaf1 = &gid_address_lcaf (a1);
1646       lcaf2 = &gid_address_lcaf (a2);
1647       if (lcaf_type (lcaf1) == lcaf_type (lcaf2))
1648         cmp = (*lcaf_cmp_fcts[lcaf_type (lcaf1)]) (lcaf1, lcaf2);
1649       break;
1650     case GID_ADDR_MAC:
1651       cmp = mac_cmp (gid_address_mac (a1), gid_address_mac (a2));
1652       break;
1653
1654     case GID_ADDR_SRC_DST:
1655       cmp = sd_cmp (&gid_address_sd (a1), &gid_address_sd (a2));
1656       break;
1657     case GID_ADDR_NSH:
1658       cmp = nsh_cmp (&gid_address_nsh (a1), &gid_address_nsh (a2));
1659       break;
1660     default:
1661       break;
1662     }
1663
1664   return cmp;
1665 }
1666
1667 u32
1668 locator_parse (void *b, locator_t * loc)
1669 {
1670   locator_hdr_t *h;
1671   u8 status = 1;                /* locator up */
1672   int len;
1673
1674   h = b;
1675   if (!LOC_REACHABLE (h) && LOC_LOCAL (h))
1676     status = 0;
1677
1678   len = gid_address_parse (LOC_ADDR (h), &loc->address);
1679   if (len == ~0)
1680     return len;
1681
1682   loc->state = status;
1683   loc->local = 0;
1684   loc->priority = LOC_PRIORITY (h);
1685   loc->weight = LOC_WEIGHT (h);
1686   loc->mpriority = LOC_MPRIORITY (h);
1687   loc->mweight = LOC_MWEIGHT (h);
1688
1689   return sizeof (locator_hdr_t) + len;
1690 }
1691
1692 void
1693 locator_copy (locator_t * dst, locator_t * src)
1694 {
1695   /* TODO if gid become more complex, this will need to be changed! */
1696   clib_memcpy (dst, src, sizeof (*dst));
1697   if (!src->local)
1698     gid_address_copy (&dst->address, &src->address);
1699 }
1700
1701 u32
1702 locator_cmp (locator_t * l1, locator_t * l2)
1703 {
1704   u32 ret = 0;
1705   if ((ret = gid_address_cmp (&l1->address, &l2->address)) != 0)
1706     return 1;
1707
1708   if (l1->priority != l2->priority)
1709     return 1;
1710   if (l1->weight != l2->weight)
1711     return 1;
1712   if (l1->mpriority != l2->mpriority)
1713     return 1;
1714   if (l1->mweight != l2->mweight)
1715     return 1;
1716   return 0;
1717 }
1718
1719 void
1720 locator_free (locator_t * l)
1721 {
1722   if (!l->local)
1723     gid_address_free (&l->address);
1724 }
1725
1726 void
1727 build_src_dst (gid_address_t * sd, gid_address_t * src, gid_address_t * dst)
1728 {
1729   memset (sd, 0, sizeof (*sd));
1730   gid_address_type (sd) = GID_ADDR_SRC_DST;
1731   gid_address_vni (sd) = gid_address_vni (dst);
1732   gid_address_vni_mask (sd) = gid_address_vni_mask (dst);
1733
1734   switch (gid_address_type (dst))
1735     {
1736     case GID_ADDR_IP_PREFIX:
1737       gid_address_sd_src_type (sd) = FID_ADDR_IP_PREF;
1738       gid_address_sd_dst_type (sd) = FID_ADDR_IP_PREF;
1739       ip_prefix_copy (&gid_address_sd_src_ippref (sd),
1740                       &gid_address_ippref (src));
1741       ip_prefix_copy (&gid_address_sd_dst_ippref (sd),
1742                       &gid_address_ippref (dst));
1743       break;
1744     case GID_ADDR_MAC:
1745       gid_address_sd_src_type (sd) = FID_ADDR_MAC;
1746       gid_address_sd_dst_type (sd) = FID_ADDR_MAC;
1747       mac_copy (gid_address_sd_src_mac (sd), gid_address_mac (src));
1748       mac_copy (gid_address_sd_dst_mac (sd), gid_address_mac (dst));
1749       break;
1750     default:
1751       clib_warning ("Unsupported gid type %d while conversion!",
1752                     gid_address_type (dst));
1753       break;
1754     }
1755 }
1756
1757 /*
1758  * fd.io coding-style-patch-verification: ON
1759  *
1760  * Local Variables:
1761  * eval: (c-set-style "gnu")
1762  * End:
1763  */