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