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