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   ip_address_set (&gid_address_ip (dst), src, version);
1257 }
1258
1259 int
1260 no_addr_cmp (void *a1, void *a2)
1261 {
1262   return 0;
1263 }
1264
1265 int
1266 vni_cmp (void *a1, void *a2)
1267 {
1268   vni_t *v1 = a1;
1269   vni_t *v2 = a2;
1270
1271   if (vni_mask_len (v1) != vni_mask_len (v2))
1272     return -1;
1273   if (vni_vni (v1) != vni_vni (v2))
1274     return -1;
1275   return gid_address_cmp (vni_gid (v1), vni_gid (v2));
1276 }
1277
1278 static int
1279 fid_addr_cmp (fid_address_t * a1, fid_address_t * a2)
1280 {
1281   if (fid_addr_type (a1) != fid_addr_type (a2))
1282     return -1;
1283
1284   switch (fid_addr_type (a1))
1285     {
1286     case FID_ADDR_IP_PREF:
1287       return ip_prefix_cmp (&fid_addr_ippref (a1), &fid_addr_ippref (a2));
1288
1289     case FID_ADDR_MAC:
1290       return memcmp (&fid_addr_mac (a1), &fid_addr_mac (a2), 6);
1291
1292     default:
1293       return -1;
1294     }
1295   return -1;
1296 }
1297
1298 int
1299 sd_cmp (void *a1, void *a2)
1300 {
1301   source_dest_t *sd1 = a1;
1302   source_dest_t *sd2 = a2;
1303
1304   if (fid_addr_cmp (&sd_dst (sd1), &sd_dst (sd2)))
1305     return -1;
1306   if (fid_addr_cmp (&sd_src (sd1), &sd_src (sd2)))
1307     return -1;
1308   return 0;
1309 }
1310
1311 /* Compare two gid_address_t.
1312  * Returns:
1313  *        -1: If they are from different afi
1314  *             0: Both address are the same
1315  *             1: Addr1 is bigger than addr2
1316  *             2: Addr2 is bigger than addr1
1317  */
1318 int
1319 gid_address_cmp (gid_address_t * a1, gid_address_t * a2)
1320 {
1321   lcaf_t *lcaf1, *lcaf2;
1322   int cmp = -1;
1323   if (!a1 || !a2)
1324     return -1;
1325   if (gid_address_type (a1) != gid_address_type (a2))
1326     return -1;
1327   if (gid_address_vni (a1) != gid_address_vni (a2))
1328     return -1;
1329   if (gid_address_vni_mask (a1) != gid_address_vni_mask (a2))
1330     return -1;
1331
1332   switch (gid_address_type (a1))
1333     {
1334     case GID_ADDR_NO_ADDRESS:
1335       if (a1 == a2)
1336         cmp = 0;
1337       else
1338         cmp = 2;
1339       break;
1340     case GID_ADDR_IP_PREFIX:
1341       cmp =
1342         ip_prefix_cmp (&gid_address_ippref (a1), &gid_address_ippref (a2));
1343       break;
1344     case GID_ADDR_LCAF:
1345       lcaf1 = &gid_address_lcaf (a1);
1346       lcaf2 = &gid_address_lcaf (a2);
1347       if (lcaf_type (lcaf1) == lcaf_type (lcaf2))
1348         cmp = (*lcaf_cmp_fcts[lcaf_type (lcaf1)]) (lcaf1, lcaf2);
1349       break;
1350     case GID_ADDR_MAC:
1351       cmp = memcmp (gid_address_mac (a1), gid_address_mac (a2),
1352                     sizeof (gid_address_mac (a1)));
1353       break;
1354
1355     case GID_ADDR_SRC_DST:
1356       cmp = sd_cmp (&gid_address_sd (a1), &gid_address_sd (a2));
1357       break;
1358     default:
1359       break;
1360     }
1361
1362   return cmp;
1363 }
1364
1365 u32
1366 locator_parse (void *b, locator_t * loc)
1367 {
1368   locator_hdr_t *h;
1369   u8 status = 1;                /* locator up */
1370   int len;
1371
1372   h = b;
1373   if (!LOC_REACHABLE (h) && LOC_LOCAL (h))
1374     status = 0;
1375
1376   len = gid_address_parse (LOC_ADDR (h), &loc->address);
1377   if (len == ~0)
1378     return len;
1379
1380   loc->state = status;
1381   loc->local = 0;
1382   loc->priority = LOC_PRIORITY (h);
1383   loc->weight = LOC_WEIGHT (h);
1384   loc->mpriority = LOC_MPRIORITY (h);
1385   loc->mweight = LOC_MWEIGHT (h);
1386
1387   return sizeof (locator_hdr_t) + len;
1388 }
1389
1390 void
1391 locator_copy (locator_t * dst, locator_t * src)
1392 {
1393   /* TODO if gid become more complex, this will need to be changed! */
1394   clib_memcpy (dst, src, sizeof (*dst));
1395   if (!src->local)
1396     gid_address_copy (&dst->address, &src->address);
1397 }
1398
1399 u32
1400 locator_cmp (locator_t * l1, locator_t * l2)
1401 {
1402   u32 ret = 0;
1403   if ((ret = gid_address_cmp (&l1->address, &l2->address)) != 0)
1404     return 1;
1405
1406   if (l1->priority != l2->priority)
1407     return 1;
1408   if (l1->weight != l2->weight)
1409     return 1;
1410   if (l1->mpriority != l2->mpriority)
1411     return 1;
1412   if (l1->mweight != l2->mweight)
1413     return 1;
1414   return 0;
1415 }
1416
1417 void
1418 locator_free (locator_t * l)
1419 {
1420   if (!l->local)
1421     gid_address_free (&l->address);
1422 }
1423
1424 /*
1425  * fd.io coding-style-patch-verification: ON
1426  *
1427  * Local Variables:
1428  * eval: (c-set-style "gnu")
1429  * End:
1430  */