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