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
  21 typedef u16 (*size_to_write_fct)(void *);
  22 typedef void * (*cast_fct)(gid_address_t *);
  23 typedef u16 (*serdes_fct)(u8 *, void *);
  24 typedef u8 (*addr_len_fct)(void *);
  25 typedef void (*copy_fct)(void *, void *);
  26 typedef void (*free_fct)(void *);
  27 typedef int (*cmp_fct)(void *, void *);
  28
  29 u16 vni_write (u8 * p, void * a);
  30 u16 vni_parse (u8 * p, void * a);
  31 u16 vni_size_to_write (void * a);
  32 void vni_free (void * a);
  33 void vni_copy (void * dst, void * src);
  34 u16 vni_length (void * a);
  35 int vni_cmp (void *, void *);
  36
  37 u16 no_addr_size_to_write (void *);
  38 u16 no_addr_write (u8 * p, void * a);
  39 u16 no_addr_parse (u8 * p, void * a);
  40 void no_addr_free (void * a);
  41 void no_addr_copy (void *, void *);
  42 u16 no_addr_length (void * a);
  43 int no_addr_cmp (void * a1, void * a2);
  44
  45 size_to_write_fct size_to_write_fcts[GID_ADDR_TYPES] =
  46   { ip_prefix_size_to_write, lcaf_size_to_write, mac_size_to_write };
  47 serdes_fct write_fcts[GID_ADDR_TYPES] =
  48   { ip_prefix_write, lcaf_write, mac_write };
  49 cast_fct cast_fcts[GID_ADDR_TYPES] =
  50   { ip_prefix_cast, lcaf_cast, mac_cast };
  51 addr_len_fct addr_len_fcts[GID_ADDR_TYPES] =
  52   { ip_prefix_length, lcaf_length, mac_length };
  53 copy_fct copy_fcts[GID_ADDR_TYPES] =
  54   { ip_prefix_copy, lcaf_copy, mac_copy };
  55
  56 cmp_fct lcaf_cmp_fcts[LCAF_TYPES] =
  57   {
  58     no_addr_cmp,
  59     NULL,
  60     vni_cmp
  61   };
  62
  63 size_to_write_fct lcaf_body_length_fcts[LCAF_TYPES] =
  64   {
  65     no_addr_length,
  66     NULL,
  67     vni_length
  68   };
  69
  70 copy_fct lcaf_copy_fcts[LCAF_TYPES] =
  71   {
  72     no_addr_copy,
  73     NULL,
  74     vni_copy
  75   };
  76
  77 free_fct lcaf_free_fcts[LCAF_TYPES] =
  78   {
  79     no_addr_free,
  80     NULL,
  81     vni_free
  82   };
  83
  84 size_to_write_fct lcaf_size_to_write_fcts[LCAF_TYPES] =
  85   {
  86     no_addr_size_to_write,
  87     NULL,
  88     vni_size_to_write
  89   };
  90
  91 serdes_fct lcaf_write_fcts[LCAF_TYPES] =
  92   {
  93     no_addr_write,
  94     NULL,
  95     vni_write
  96   };
  97
  98 serdes_fct lcaf_parse_fcts[LCAF_TYPES] =
  99   {
 100     no_addr_parse,
 101     NULL,
 102     vni_parse
 103   };
 104
 105 u8 *
 106 format_ip_address (u8 * s, va_list * args)
 107 {
 108   ip_address_t * a = va_arg (*args, ip_address_t *);
 109   u8 ver = ip_addr_version(a);
 110   if (ver == IP4)
 111     {
 112       return format (s, "%U", format_ip4_address, &ip_addr_v4(a));
 113     }
 114   else if (ver == IP6)
 115     {
 116       return format (s, "%U", format_ip6_address, &ip_addr_v6(a));
 117     }
 118   else
 119     {
 120       clib_warning ("Can't format IP version %d!", ver);
 121       return 0;
 122     }
 123 }
 124
 125 uword
 126 unformat_ip_address (unformat_input_t * input, va_list * args)
 127 {
 128   ip_address_t * a = va_arg(*args, ip_address_t *);
 129   if (unformat(input, "%U", unformat_ip4_address, &ip_addr_v4(a)))
 130     ip_addr_version(a) = IP4;
 131   else if (unformat_user (input, unformat_ip6_address, &ip_addr_v6(a)))
 132     ip_addr_version(a) = IP6;
 133   else
 134     return 0;
 135   return 1;
 136 }
 137
 138 u8 *
 139 format_ip_prefix (u8 * s, va_list * args)
 140 {
 141   ip_prefix_t * a = va_arg (*args, ip_prefix_t *);
 142   return format (s, "%U/%d", format_ip_address, &ip_prefix_addr(a), ip_prefix_len(a));
 143 }
 144
 145 uword
 146 unformat_ip_prefix (unformat_input_t * input, va_list * args)
 147 {
 148   ip_prefix_t * a = va_arg(*args, ip_prefix_t *);
 149   if (unformat (input, "%U/%d", unformat_ip_address, &ip_prefix_addr(a),
 150                    &ip_prefix_len(a)))
 151     {
 152       if ((ip_prefix_version(a) == IP4 && 32 < ip_prefix_len(a)) ||
 153           (ip_prefix_version(a) == IP6 && 128 < ip_prefix_length(a)))
 154         {
 155           clib_warning("Prefix length to big: %d!", ip_prefix_len(a));
 156           return 0;
 157         }
 158       ip_prefix_normalize(a);
 159     }
 160   else
 161       return 0;
 162   return 1;
 163 }
 164
 165 uword
 166 unformat_mac_address (unformat_input_t * input, va_list * args)
 167 {
 168   u8 * a = va_arg(*args, u8 *);
 169   return unformat (input, "%x:%x:%x:%x:%x:%x", &a[0], &a[1], &a[2], &a[3],
 170                    &a[4], &a[5]);
 171 }
 172
 173 u8 *
 174 format_mac_address (u8 * s, va_list * args)
 175 {
 176   u8 * a = va_arg (*args, u8 *);
 177   return format (s, "%02x:%02x:%02x:%02x:%02x:%02x",
 178                  a[0], a[1], a[2], a[3], a[4], a[5]);
 179 }
 180
 181 u8 *
 182 format_gid_address (u8 * s, va_list * args)
 183 {
 184   gid_address_t * a = va_arg(*args, gid_address_t *);
 185   u8 type = gid_address_type(a);
 186   switch (type)
 187     {
 188     case GID_ADDR_IP_PREFIX:
 189       return format (s, "[%d] %U", gid_address_vni(a), format_ip_prefix,
 190                      &gid_address_ippref(a));
 191     case GID_ADDR_SRC_DST:
 192       return format (s, "[%d] %U|%U", gid_address_vni(a),
 193                      format_ip_prefix, &gid_address_sd_source_pref(a),
 194                      format_ip_prefix, &gid_address_sd_dest_pref(a));
 195     case GID_ADDR_MAC:
 196       return format (s, "[%d] %U", gid_address_vni(a), format_mac_address,
 197                      &gid_address_mac(a));
 198     default:
 199       clib_warning("Can't format gid type %d", type);
 200       return 0;
 201     }
 202 }
 203
 204 uword
 205 unformat_gid_address (unformat_input_t * input, va_list * args)
 206 {
 207   u32 vni;
 208   gid_address_t * a = va_arg(*args, gid_address_t *);
 209   u8 mac[6] = {0};
 210   ip_prefix_t ippref;
 211
 212   memset (&ippref, 0, sizeof (ippref));
 213   memset(a, 0, sizeof(a[0]));
 214
 215   if (unformat (input, "%U", unformat_ip_prefix, &ippref))
 216     {
 217       clib_memcpy (&gid_address_ippref(a), &ippref, sizeof(ippref));
 218       gid_address_type(a) = GID_ADDR_IP_PREFIX;
 219     }
 220   else if (unformat (input, "%U", unformat_mac_address, mac))
 221     {
 222       clib_memcpy (gid_address_mac(a), mac, sizeof(mac));
 223       gid_address_type(a) = GID_ADDR_MAC;
 224     }
 225   else if (unformat (input, "[%d]", &vni))
 226     gid_address_vni(a) = vni;
 227   else
 228     return 0;
 229
 230   return 1;
 231 }
 232
 233 uword
 234 unformat_negative_mapping_action (unformat_input_t * input, va_list * args)
 235 {
 236   u32 * action = va_arg(*args, u32 *);
 237   u8 * s = 0;
 238
 239   if (unformat (input, "%s", &s))
 240     {
 241       int len = vec_len(s);
 242       clib_warning ("len = %d", len);
 243       if (!strcmp ((char *) s, "no-action"))
 244         action[0] = ACTION_NONE;
 245       if (!strcmp ((char *) s, "natively-forward"))
 246         action[0] = ACTION_NATIVELY_FORWARDED;
 247       if (!strcmp ((char *) s, "send-map-request"))
 248         action[0] = ACTION_SEND_MAP_REQUEST;
 249       else if (!strcmp ((char *) s, "drop"))
 250         action[0] = ACTION_DROP;
 251       else
 252         {
 253           clib_warning("invalid action: '%s'", s);
 254           action[0] = ACTION_DROP;
 255           return 0;
 256         }
 257     }
 258   else
 259     return 0;
 260
 261   return 1;
 262 }
 263
 264 u16
 265 ip_address_size (ip_address_t * a)
 266 {
 267   switch (ip_addr_version (a))
 268   {
 269     case IP4:
 270       return sizeof(ip4_address_t);
 271       break;
 272     case IP6:
 273       return sizeof(ip6_address_t);
 274       break;
 275   }
 276   return 0;
 277 }
 278
 279 u16
 280 ip_version_to_size (u8 ver)
 281 {
 282   switch (ver)
 283   {
 284     case IP4:
 285       return sizeof(ip4_address_t);
 286       break;
 287     case IP6:
 288       return sizeof(ip6_address_t);
 289       break;
 290   }
 291   return 0;
 292 }
 293
 294 u8
 295 ip_version_to_max_plen (u8 ver)
 296 {
 297   switch (ver)
 298   {
 299     case IP4:
 300       return 32;
 301       break;
 302     case IP6:
 303       return 128;
 304       break;
 305   }
 306   return 0;
 307 }
 308
 309 always_inline lisp_afi_e
 310 ip_version_to_iana_afi (u16 version)
 311 {
 312   switch (version)
 313     {
 314     case IP4:
 315       return LISP_AFI_IP;
 316     case IP6:
 317       return LISP_AFI_IP6;
 318     default:
 319       return 0;
 320     }
 321   return 0;
 322 }
 323
 324 always_inline u8
 325 ip_iana_afi_to_version (lisp_afi_e afi)
 326 {
 327   switch (afi)
 328     {
 329     case LISP_AFI_IP:
 330       return IP4;
 331     case LISP_AFI_IP6:
 332       return IP6;
 333     default:
 334       return 0;
 335     }
 336   return 0;
 337 }
 338
 339 u16
 340 ip_address_size_to_write (ip_address_t * a)
 341 {
 342   return ip_address_size (a) + sizeof (u16);
 343 }
 344
 345 u16
 346 ip_address_iana_afi(ip_address_t *a)
 347 {
 348     return ip_version_to_iana_afi(ip_addr_version(a));
 349 }
 350
 351 u8
 352 ip_address_max_len (u8 version)
 353 {
 354   return version == IP4 ? 32 : 128;
 355 }
 356
 357 u16
 358 ip4_address_size_to_put ()
 359 {
 360   // return sizeof(u16) + sizeof (ip4_address_t);
 361   return 6;
 362 }
 363
 364 u16
 365 ip6_address_size_to_put ()
 366 {
 367   //return sizeof(u16) + sizeof (ip6_address_t);
 368   return 18;
 369 }
 370
 371 u32
 372 ip4_address_put (u8 * b, ip4_address_t * a)
 373 {
 374   *(u16 *)b = clib_host_to_net_u16(ip_version_to_iana_afi(IP4));
 375   u8 *p = b + sizeof (u16);
 376   clib_memcpy (p, a, sizeof(*a));
 377   return ip4_address_size_to_put();
 378 }
 379
 380 u32
 381 ip6_address_put (u8 * b, ip6_address_t * a)
 382 {
 383   *(u16 *)b = clib_host_to_net_u16(ip_version_to_iana_afi(IP6));
 384   u8 *p = b + sizeof (u16);
 385   clib_memcpy (p, a, sizeof(*a));
 386   return ip6_address_size_to_put();
 387 }
 388
 389 u32
 390 ip_address_put (u8 * b, ip_address_t * a)
 391 {
 392   u32 len = ip_address_size (a);
 393   *(u16 *) b = clib_host_to_net_u16(ip_address_iana_afi (a));
 394   u8 * p = b + sizeof (u16);
 395   clib_memcpy (p, &ip_addr_addr (a), len);
 396   return (len + sizeof (u16));
 397 }
 398
 399 u32
 400 ip_address_parse(void * offset, u16 iana_afi, ip_address_t *dst)
 401 {
 402   ip_addr_version(dst) = ip_iana_afi_to_version (iana_afi);
 403   u8 size = ip_version_to_size (ip_addr_version(dst));
 404   clib_memcpy (&ip_addr_addr(dst), offset + sizeof(u16), size);
 405   return(sizeof(u16) + size);
 406 }
 407
 408 u32
 409 lcaf_hdr_parse (void * offset, lcaf_t * lcaf)
 410 {
 411   lcaf_hdr_t * lh = offset;
 412   lcaf->type = lh->type;
 413
 414   /* this is a bit of hack: since the LCAF Instance ID is the
 415     only message that uses reserved2 field, we can set it here.
 416     If any LCAF format starts using reserved2 field as well this needs
 417     to be moved elsewhere */
 418   lcaf_vni_len (lcaf) = lh->reserved2;
 419
 420   return sizeof (lh[0]);
 421 }
 422
 423 u16
 424 vni_parse (u8 * p, void * a)
 425 {
 426   lcaf_t * lcaf = a;
 427   gid_address_t * g = a;
 428   u16 size = 0;
 429
 430   gid_address_vni (g) = clib_net_to_host_u32 ( *(u32 *) p);
 431   size += sizeof (u32);
 432   gid_address_vni_mask (g) = lcaf_vni_len (lcaf);
 433
 434   /* nested LCAFs are not supported - it is expected that nested AFI type is
 435      IP address */
 436   size += gid_address_parse (p + size, g);
 437   return size;
 438 }
 439
 440 u16
 441 no_addr_parse (u8 * p, void * a)
 442 {
 443   /* do nothing */
 444   return 0;
 445 }
 446
 447 u32
 448 lcaf_parse (void * offset, gid_address_t *addr)
 449 {
 450   /* skip AFI type */
 451   offset += sizeof (u16);
 452   lcaf_t * lcaf = &gid_address_lcaf (addr);
 453
 454   u32 size = lcaf_hdr_parse (offset, lcaf);
 455   u8 type = lcaf_type (lcaf);
 456
 457   if (!lcaf_parse_fcts[type])
 458     {
 459       clib_warning ("Unsupported LCAF type: %u", type);
 460       return ~0;
 461     }
 462   size += (*lcaf_parse_fcts[type])(offset + size, lcaf);
 463   return sizeof (u16) + size;
 464 }
 465
 466 void
 467 vni_free (void * a)
 468 {
 469   vni_t * v = a;
 470   gid_address_free (vni_gid (v));
 471   clib_mem_free (vni_gid (v));
 472 }
 473
 474 void
 475 no_addr_free (void * a)
 476 {
 477   /* nothing to do */
 478 }
 479
 480 void
 481 gid_address_free (gid_address_t *a)
 482 {
 483   if (gid_address_type (a) != GID_ADDR_LCAF)
 484     return;
 485
 486   lcaf_t * lcaf = &gid_address_lcaf (a);
 487   u8 lcaf_type = lcaf_type (lcaf);
 488   (*lcaf_free_fcts[lcaf_type])(lcaf);
 489 }
 490
 491 int
 492 ip_address_cmp (ip_address_t * ip1, ip_address_t * ip2)
 493 {
 494   int res = 0;
 495   if (ip_addr_version (ip1) != ip_addr_version(ip2))
 496     return -1;
 497   res = memcmp (&ip_addr_addr(ip1), &ip_addr_addr(ip2), ip_address_size (ip1));
 498
 499   if (res < 0)
 500     res = 2;
 501   else if (res > 0)
 502     res = 1;
 503
 504   return res;
 505 }
 506
 507 void
 508 ip_address_copy (ip_address_t * dst , ip_address_t * src)
 509 {
 510   clib_memcpy (dst, src, sizeof (ip_address_t));
 511 }
 512
 513 void
 514 ip_address_copy_addr (void * dst , ip_address_t * src)
 515 {
 516   clib_memcpy (dst, src, ip_address_size(src));
 517 }
 518
 519 void
 520 ip_address_set(ip_address_t * dst, void * src, u8 version)
 521 {
 522   clib_memcpy(dst, src, ip_version_to_size(version));
 523   ip_addr_version(dst) = version;
 524 }
 525
 526 static void
 527 ip_prefix_normalize_ip4 (ip4_address_t * ip4, u8 preflen)
 528 {
 529   u32 mask = ~0;
 530
 531   ASSERT (ip4);
 532
 533   if (32 <= preflen)
 534    {
 535      return;
 536    }
 537
 538   mask = pow2_mask (preflen) << (32 - preflen);
 539   mask = clib_host_to_net_u32 (mask);
 540   ip4->data_u32 &= mask;
 541 }
 542
 543 static void
 544 ip_prefix_normalize_ip6 (ip6_address_t * ip6, u8 preflen)
 545 {
 546   u8 mask_6[16];
 547   u32 * m;
 548   u8 j ,i0, i1;
 549
 550   ASSERT (ip6);
 551
 552   memset (mask_6, 0, sizeof (mask_6));
 553
 554   if (128 <= preflen)
 555    {
 556      return;
 557    }
 558
 559   i1 = preflen % 32;
 560   i0 = preflen / 32;
 561   m = (u32 * ) &mask_6[0];
 562
 563   for (j = 0; j < i0; j++)
 564     {
 565       m[j] = ~0;
 566     }
 567
 568   if (i1)
 569    {
 570      m[i0] = clib_host_to_net_u32 (pow2_mask(i1) << (32 - i1));
 571    }
 572
 573   for (j = 0; j < sizeof(mask_6); j++)
 574     {
 575       ip6->as_u8[j] &= mask_6[j];
 576     }
 577 }
 578
 579 void
 580 ip_prefix_normalize(ip_prefix_t * a)
 581 {
 582   u8 preflen = ip_prefix_len(a);
 583
 584   switch (ip_prefix_version (a))
 585   {
 586     case IP4:
 587       ip_prefix_normalize_ip4(&ip_prefix_v4(a), preflen);
 588       break;
 589
 590     case IP6:
 591       ip_prefix_normalize_ip6(&ip_prefix_v6(a), preflen);
 592       break;
 593
 594     default:
 595       ASSERT(0);
 596   }
 597 }
 598
 599 void *
 600 ip_prefix_cast (gid_address_t * a)
 601 {
 602   return &gid_address_ippref(a);
 603 }
 604
 605 u16
 606 ip_prefix_size_to_write (void * pref)
 607 {
 608   ip_prefix_t *a = (ip_prefix_t *) pref;
 609   return ip_address_size_to_write (&ip_prefix_addr (a));
 610 }
 611
 612 u16
 613 ip_prefix_write (u8 * p, void * gid)
 614 {
 615   gid_address_t * g = gid;
 616   ip_prefix_t *a = &gid_address_ippref (g);
 617
 618   switch (ip_prefix_version (a))
 619   {
 620     case IP4:
 621       return ip4_address_put (p, &ip_prefix_v4 (a));
 622       break;
 623     case IP6:
 624       return ip6_address_put (p, &ip_prefix_v6 (a));
 625       break;
 626   }
 627   return 0;
 628 }
 629
 630 u8
 631 ip_prefix_length (void *a)
 632 {
 633   return ip_prefix_len((ip_prefix_t *) a);
 634 }
 635
 636 void
 637 ip_prefix_copy (void * dst , void * src)
 638 {
 639   clib_memcpy (dst, src, sizeof (ip_prefix_t));
 640 }
 641
 642 void
 643 mac_copy (void * dst , void * src)
 644 {
 645   clib_memcpy (dst, src, 6);
 646 }
 647
 648 int
 649 ip_prefix_cmp(ip_prefix_t * p1, ip_prefix_t * p2)
 650 {
 651   int cmp = 0;
 652
 653   ip_prefix_normalize (p1);
 654   ip_prefix_normalize (p2);
 655
 656   cmp = ip_address_cmp (&ip_prefix_addr(p1), &ip_prefix_addr(p2));
 657   if (cmp == 0)
 658   {
 659     if (ip_prefix_len(p1) < ip_prefix_len(p2))
 660     {
 661       cmp = 1;
 662     }
 663     else
 664     {
 665       if (ip_prefix_len(p1) > ip_prefix_len(p2))
 666         cmp = 2;
 667     }
 668   }
 669   return cmp;
 670 }
 671
 672 void
 673 no_addr_copy (void * dst, void * src)
 674 {
 675   /* nothing to do */
 676 }
 677
 678 void
 679 vni_copy (void * dst, void * src)
 680 {
 681   vni_t * vd = dst;
 682   vni_t * vs = src;
 683
 684   clib_memcpy (vd, vs, sizeof (vd[0]));
 685   vni_gid (vd) = clib_mem_alloc (sizeof (gid_address_t));
 686   gid_address_copy (vni_gid (vd), vni_gid (vs));
 687 }
 688
 689 void
 690 lcaf_copy (void * dst , void * src)
 691 {
 692   lcaf_t * lcaf_dst = dst;
 693   lcaf_t * lcaf_src = src;
 694
 695   lcaf_type (lcaf_dst) = lcaf_type (lcaf_src);
 696   (*lcaf_copy_fcts[lcaf_type (lcaf_src)])(dst, src);
 697 }
 698
 699 u8
 700 lcaf_length (void *a)
 701 {
 702   return 0;
 703 }
 704
 705 u8
 706 mac_length (void *a)
 707 {
 708   return 0;
 709 }
 710
 711 void *
 712 lcaf_cast (gid_address_t * a)
 713 {
 714   return &gid_address_lcaf (a);
 715 }
 716
 717 void *
 718 mac_cast (gid_address_t * a)
 719 {
 720   return &gid_address_mac (a);
 721 }
 722
 723 u16
 724 no_addr_length (void * a)
 725 {
 726   return 0;
 727 }
 728
 729 u16
 730 vni_length (void * a)
 731 {
 732   vni_t * v = a;
 733   return (sizeof (u32) /* VNI size */
 734    + gid_address_size_to_put (vni_gid (v)) /* vni body size*/);
 735 }
 736
 737 u16
 738 lcaf_write (u8 * p, void * a)
 739 {
 740   u16 size = 0, len;
 741   lcaf_t * lcaf = a;
 742   u8 type = lcaf_type (lcaf);
 743   lcaf_hdr_t _h, *h = &_h;
 744
 745   *(u16 *) p = clib_host_to_net_u16 (LISP_AFI_LCAF);
 746   size += sizeof (u16);
 747   memset (h, 0, sizeof (h[0]));
 748   LCAF_TYPE (h) = type;
 749   u16 lcaf_len = (*lcaf_body_length_fcts[type])(lcaf);
 750   LCAF_LENGTH (h) = clib_host_to_net_u16 (lcaf_len);
 751
 752   clib_memcpy (p + size, h, sizeof (h[0]));
 753   size += sizeof (h[0]);
 754   len = (*lcaf_write_fcts[type])(p + size, lcaf);
 755
 756   if ((u16)~0 == len)
 757     return ~0;
 758
 759   return size + len;
 760 }
 761
 762 u16
 763 mac_write (u8 * p, void * a)
 764 {
 765   *(u16 *)p = clib_host_to_net_u16 (LISP_AFI_MAC);
 766   clib_memcpy(p + sizeof (u16), a, 6);
 767   return mac_size_to_write (a);
 768 }
 769
 770 u16
 771 vni_write (u8 * p, void * a)
 772 {
 773   lcaf_hdr_t _h, *h = &_h;
 774   gid_address_t * g = a;
 775   u16 size = 0, len;
 776
 777   /* put lcaf header */
 778   *(u16 *) p = clib_host_to_net_u16 (LISP_AFI_LCAF);
 779   size += sizeof (u16);
 780   memset (h, 0, sizeof (h[0]));
 781   LCAF_TYPE (h) = LCAF_INSTANCE_ID;
 782   u16 lcaf_len = sizeof (u32) /* Instance ID size */
 783     + gid_address_size_to_put_no_vni (g);
 784   LCAF_LENGTH (h) = clib_host_to_net_u16 (lcaf_len);
 785   LCAF_RES2 (h) = gid_address_vni_mask (g);
 786
 787   /* put vni header */
 788   clib_memcpy (p + size, h, sizeof (h[0]));
 789   size += sizeof (h[0]);
 790
 791   u32 * afip = (u32 *)(p + size);
 792   afip[0] = clib_host_to_net_u32 (gid_address_vni (g));
 793   size += sizeof (u32);
 794
 795   /* write the actual address */
 796   len = gid_address_put_no_vni (p + size, g);
 797
 798   if ((u16)~0 == len)
 799     return ~0;
 800
 801   return size + len;
 802 }
 803
 804 u16
 805 no_addr_write (u8 * p, void * a)
 806 {
 807   /* do nothing; return AFI field size */
 808   return sizeof (u16);
 809 }
 810
 811 u16
 812 no_addr_size_to_write (void * a)
 813 {
 814   return sizeof (u16); /* AFI field length */
 815 }
 816
 817 u16
 818 vni_size_to_write (void * a)
 819 {
 820   gid_address_t * g =  a;
 821   return (sizeof (u32) /* vni size */
 822           + sizeof (u16) /* LCAF AFI field size */
 823           + sizeof (lcaf_hdr_t)
 824     + gid_address_size_to_put_no_vni (g));
 825 }
 826
 827 u16
 828 lcaf_size_to_write (void * a)
 829 {
 830   lcaf_t * lcaf = (lcaf_t *) a;
 831   u32 size = 0, len;
 832   u8 type = lcaf_type (lcaf);
 833
 834   size += sizeof (u16); /* AFI size */
 835
 836   len = (*lcaf_size_to_write_fcts[type])(lcaf);
 837   if (~0 == len)
 838     return ~0;
 839
 840   return size + len;
 841 }
 842
 843 u16
 844 mac_size_to_write (void * a)
 845 {
 846   return sizeof (u16) + 6;
 847 }
 848
 849 u8
 850 gid_address_len (gid_address_t *a)
 851 {
 852   gid_address_type_t type = gid_address_type (a);
 853   return (*addr_len_fcts[type])((*cast_fcts[type])(a));
 854 }
 855
 856 static u16
 857 gid_address_put_no_vni (u8 * b, gid_address_t * gid)
 858 {
 859   gid_address_type_t type = gid_address_type (gid);
 860   return (*write_fcts[type])(b, (*cast_fcts[type])(gid));
 861 }
 862
 863 u16
 864 gid_address_put (u8 * b, gid_address_t * gid)
 865 {
 866   if (0 != gid_address_vni (gid))
 867     return vni_write (b, gid);
 868
 869   return gid_address_put_no_vni (b, gid);
 870 }
 871
 872 static u16
 873 gid_address_size_to_put_no_vni (gid_address_t * gid)
 874 {
 875   gid_address_type_t type = gid_address_type (gid);
 876   return (*size_to_write_fcts[type])((*cast_fcts[type])(gid));
 877 }
 878
 879 u16
 880 gid_address_size_to_put (gid_address_t * gid)
 881 {
 882   if (0 != gid_address_vni (gid))
 883     return vni_size_to_write (gid);
 884
 885   return gid_address_size_to_put_no_vni (gid);
 886 }
 887
 888 void *
 889 gid_address_cast (gid_address_t * gid, gid_address_type_t type)
 890 {
 891   return (*cast_fcts[type])(gid);
 892 }
 893
 894 void
 895 gid_address_copy(gid_address_t * dst, gid_address_t * src)
 896 {
 897   gid_address_type_t type = gid_address_type(src);
 898   (*copy_fcts[type])((*cast_fcts[type])(dst), (*cast_fcts[type])(src));
 899   gid_address_type(dst) = type;
 900   gid_address_vni(dst) = gid_address_vni(src);
 901   gid_address_vni_mask(dst) = gid_address_vni_mask(src);
 902 }
 903
 904 u32
 905 mac_parse (u8 * offset, gid_address_t * a)
 906 {
 907   /* skip AFI field */
 908   offset += sizeof (u16);
 909
 910   memcpy (gid_address_mac (a), offset, sizeof (gid_address_mac (a)));
 911   return (sizeof (u16) + sizeof (gid_address_mac (a)));
 912 }
 913
 914 u32
 915 gid_address_parse (u8 * offset, gid_address_t *a)
 916 {
 917   lisp_afi_e afi;
 918   int len = 0;
 919
 920   if (!a)
 921     return 0;
 922
 923   /* NOTE: since gid_adress_parse may be called by vni_parse, we can't 0
 924    * the gid address here */
 925   afi = clib_net_to_host_u16 (*((u16 *) offset));
 926
 927   switch (afi)
 928     {
 929     case LISP_AFI_NO_ADDR:
 930       len = sizeof(u16);
 931       gid_address_type(a) = GID_ADDR_NO_ADDRESS;
 932       break;
 933     case LISP_AFI_IP:
 934       len = ip_address_parse (offset, afi, &gid_address_ip(a));
 935       gid_address_type(a) = GID_ADDR_IP_PREFIX;
 936       /* this should be modified outside if needed*/
 937       gid_address_ippref_len(a) = 32;
 938       break;
 939     case LISP_AFI_IP6:
 940       len = ip_address_parse (offset, afi, &gid_address_ip(a));
 941       gid_address_type(a) = GID_ADDR_IP_PREFIX;
 942       /* this should be modified outside if needed*/
 943       gid_address_ippref_len(a) = 128;
 944       break;
 945     case LISP_AFI_LCAF:
 946       gid_address_type(a) = GID_ADDR_LCAF;
 947       len = lcaf_parse (offset, a);
 948       break;
 949     case LISP_AFI_MAC:
 950       len = mac_parse (offset, a);
 951       gid_address_type(a) = GID_ADDR_MAC;
 952       break;
 953     default:
 954       clib_warning("LISP AFI %d not supported!", afi);
 955       return ~0;
 956     }
 957   return len;
 958 }
 959
 960 int
 961 no_addr_cmp (void * a1, void * a2)
 962 {
 963   return 0;
 964 }
 965
 966 int
 967 vni_cmp (void * a1, void * a2)
 968 {
 969   vni_t * v1 = a1;
 970   vni_t * v2 = a2;
 971
 972   if (vni_mask_len (v1) != vni_mask_len (v2))
 973     return -1;
 974   if (vni_vni (v1) != vni_vni (v2))
 975     return -1;
 976   return gid_address_cmp (vni_gid (v1), vni_gid (v2));
 977 }
 978
 979 /* Compare two gid_address_t.
 980  * Returns:
 981  *        -1: If they are from different afi
 982  *             0: Both address are the same
 983  *             1: Addr1 is bigger than addr2
 984  *             2: Addr2 is bigger than addr1
 985  */
 986 int
 987 gid_address_cmp (gid_address_t * a1, gid_address_t * a2)
 988 {
 989   lcaf_t * lcaf1, * lcaf2;
 990   int cmp = -1;
 991   if (!a1 || !a2)
 992     return -1;
 993   if (gid_address_type(a1) != gid_address_type(a2))
 994     return -1;
 995   if (gid_address_vni(a1) != gid_address_vni(a2))
 996     return -1;
 997   if (gid_address_vni_mask(a1) != gid_address_vni_mask(a2))
 998     return -1;
 999
1000   switch (gid_address_type(a1))
1001     {
1002     case GID_ADDR_NO_ADDRESS:
1003       if (a1 == a2)
1004         cmp = 0;
1005       else
1006         cmp = 2;
1007       break;
1008     case GID_ADDR_IP_PREFIX:
1009       cmp = ip_prefix_cmp (&gid_address_ippref(a1), &gid_address_ippref(a2));
1010       break;
1011     case GID_ADDR_LCAF:
1012       lcaf1 = &gid_address_lcaf (a1);
1013       lcaf2 = &gid_address_lcaf (a2);
1014       if (lcaf_type (lcaf1) == lcaf_type (lcaf2))
1015         cmp = (*lcaf_cmp_fcts[lcaf_type (lcaf1)])(lcaf1, lcaf2);
1016       break;
1017     case GID_ADDR_MAC:
1018       cmp = memcmp (gid_address_mac (a1), gid_address_mac (a2),
1019                     sizeof (gid_address_mac (a1)));
1020       break;
1021     default:
1022       break;
1023     }
1024
1025   return cmp;
1026 }
1027
1028
1029 u32
1030 locator_parse (void * b, locator_t * loc)
1031 {
1032   locator_hdr_t * h;
1033   u8 status = 1; /* locator up */
1034   int len;
1035
1036   h = b;
1037   if (!LOC_REACHABLE(h) && LOC_LOCAL(h))
1038     status = 0;
1039
1040   len = gid_address_parse (LOC_ADDR(h), &loc->address);
1041   if (len == ~0)
1042     return len;
1043
1044   loc->state = status;
1045   loc->local = 0;
1046   loc->priority = LOC_PRIORITY(h);
1047   loc->weight = LOC_WEIGHT(h);
1048   loc->mpriority = LOC_MPRIORITY(h);
1049   loc->mweight = LOC_MWEIGHT(h);
1050
1051   return sizeof(locator_hdr_t) + len;
1052 }
1053
1054 void
1055 locator_copy (locator_t * dst, locator_t * src)
1056 {
1057   /* TODO if gid become more complex, this will need to be changed! */
1058   clib_memcpy (dst, src, sizeof(*dst));
1059   if (!src->local)
1060     gid_address_copy (&dst->address, &src->address);
1061 }
1062
1063 u32
1064 locator_cmp (locator_t * l1, locator_t * l2)
1065 {
1066   u32 ret = 0;
1067   if ((ret = gid_address_cmp (&l1->address, &l2->address)) != 0)
1068     return 1;
1069
1070   if (l1->priority != l2->priority)
1071     return 1;
1072   if (l1->weight != l2->weight)
1073     return 1;
1074   if (l1->mpriority != l2->mpriority)
1075     return 1;
1076   if (l1->mweight != l2->mweight)
1077     return 1;
1078   return 0;
1079 }
1080
1081 void
1082 locator_free (locator_t * l)
1083 {
1084   if (!l->local)
1085     gid_address_free (&l->address);
1086 }