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