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