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