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