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