src/vnet/lisp-cp/lisp_types.c

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