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 void
 527 ip_prefix_normalize(ip_prefix_t * a)
 528 {
 529   ip_address_t * ip;
 530   ip4_address_t * ip4;
 531   ip6_address_t * ip6;
 532   int preflen = ip_prefix_len(a);
 533   u32 mask = ~0;
 534   u64 mask_6[2];
 535   u32 * m;
 536   u32 j, i0, i1;
 537
 538   ip = &ip_prefix_addr (a);
 539   switch (ip_addr_version (ip))
 540   {
 541     case IP4:
 542       if (32 <= preflen)
 543         {
 544           break;
 545         }
 546
 547       ip4 = &ip_addr_v4 (ip);
 548       mask = pow2_mask (preflen) << (32 - preflen);
 549       mask = clib_host_to_net_u32 (mask);
 550       ip4->data_u32 &= mask;
 551       break;
 552
 553     case IP6:
 554       if (128 <= preflen)
 555         {
 556           break;
 557         }
 558       ip6 = &ip_addr_v6 (ip);
 559       memset(mask_6, 0, sizeof(mask_6));
 560       m = (u32 * ) mask_6;
 561
 562       i0 = preflen / 32;
 563       i1 = preflen % 32;
 564       for (j = 0; j < i0; j++)
 565         {
 566           m[j] = ~0;
 567         }
 568
 569       if (i1)
 570         {
 571           m[i0] = clib_host_to_net_u32 (pow2_mask(i1) << (32 - i1));
 572         }
 573
 574       ip6->as_u64[0] &= mask_6[0];
 575       ip6->as_u64[1] &= mask_6[1];
 576       break;
 577
 578     default:
 579       ASSERT(0);
 580   }
 581 }
 582
 583 void *
 584 ip_prefix_cast (gid_address_t * a)
 585 {
 586   return &gid_address_ippref(a);
 587 }
 588
 589 u16
 590 ip_prefix_size_to_write (void * pref)
 591 {
 592   ip_prefix_t *a = (ip_prefix_t *) pref;
 593   return ip_address_size_to_write (&ip_prefix_addr (a));
 594 }
 595
 596 u16
 597 ip_prefix_write (u8 * p, void * gid)
 598 {
 599   gid_address_t * g = gid;
 600   ip_prefix_t *a = &gid_address_ippref (g);
 601
 602   switch (ip_prefix_version (a))
 603   {
 604     case IP4:
 605       return ip4_address_put (p, &ip_prefix_v4 (a));
 606       break;
 607     case IP6:
 608       return ip6_address_put (p, &ip_prefix_v6 (a));
 609       break;
 610   }
 611   return 0;
 612 }
 613
 614 u8
 615 ip_prefix_length (void *a)
 616 {
 617   return ip_prefix_len((ip_prefix_t *) a);
 618 }
 619
 620 void
 621 ip_prefix_copy (void * dst , void * src)
 622 {
 623   clib_memcpy (dst, src, sizeof (ip_prefix_t));
 624 }
 625
 626 void
 627 mac_copy (void * dst , void * src)
 628 {
 629   clib_memcpy (dst, src, 6);
 630 }
 631
 632 int
 633 ip_prefix_cmp(ip_prefix_t * p1, ip_prefix_t * p2)
 634 {
 635   int cmp = 0;
 636
 637   ip_prefix_normalize (p1);
 638   ip_prefix_normalize (p2);
 639
 640   cmp = ip_address_cmp (&ip_prefix_addr(p1), &ip_prefix_addr(p2));
 641   if (cmp == 0)
 642   {
 643     if (ip_prefix_len(p1) < ip_prefix_len(p2))
 644     {
 645       cmp = 1;
 646     }
 647     else
 648     {
 649       if (ip_prefix_len(p1) > ip_prefix_len(p2))
 650         cmp = 2;
 651     }
 652   }
 653   return cmp;
 654 }
 655
 656 void
 657 no_addr_copy (void * dst, void * src)
 658 {
 659   /* nothing to do */
 660 }
 661
 662 void
 663 vni_copy (void * dst, void * src)
 664 {
 665   vni_t * vd = dst;
 666   vni_t * vs = src;
 667
 668   clib_memcpy (vd, vs, sizeof (vd[0]));
 669   vni_gid (vd) = clib_mem_alloc (sizeof (gid_address_t));
 670   gid_address_copy (vni_gid (vd), vni_gid (vs));
 671 }
 672
 673 void
 674 lcaf_copy (void * dst , void * src)
 675 {
 676   lcaf_t * lcaf_dst = dst;
 677   lcaf_t * lcaf_src = src;
 678
 679   lcaf_type (lcaf_dst) = lcaf_type (lcaf_src);
 680   (*lcaf_copy_fcts[lcaf_type (lcaf_src)])(dst, src);
 681 }
 682
 683 u8
 684 lcaf_length (void *a)
 685 {
 686   return 0;
 687 }
 688
 689 u8
 690 mac_length (void *a)
 691 {
 692   return 0;
 693 }
 694
 695 void *
 696 lcaf_cast (gid_address_t * a)
 697 {
 698   return &gid_address_lcaf (a);
 699 }
 700
 701 void *
 702 mac_cast (gid_address_t * a)
 703 {
 704   return &gid_address_mac (a);
 705 }
 706
 707 u16
 708 no_addr_length (void * a)
 709 {
 710   return 0;
 711 }
 712
 713 u16
 714 vni_length (void * a)
 715 {
 716   vni_t * v = a;
 717   return (sizeof (u32) /* VNI size */
 718    + gid_address_size_to_put (vni_gid (v)) /* vni body size*/);
 719 }
 720
 721 u16
 722 lcaf_write (u8 * p, void * a)
 723 {
 724   u16 size = 0, len;
 725   lcaf_t * lcaf = a;
 726   u8 type = lcaf_type (lcaf);
 727   lcaf_hdr_t _h, *h = &_h;
 728
 729   *(u16 *) p = clib_host_to_net_u16 (LISP_AFI_LCAF);
 730   size += sizeof (u16);
 731   memset (h, 0, sizeof (h[0]));
 732   LCAF_TYPE (h) = type;
 733   u16 lcaf_len = (*lcaf_body_length_fcts[type])(lcaf);
 734   LCAF_LENGTH (h) = clib_host_to_net_u16 (lcaf_len);
 735
 736   clib_memcpy (p + size, h, sizeof (h[0]));
 737   size += sizeof (h[0]);
 738   len = (*lcaf_write_fcts[type])(p + size, lcaf);
 739
 740   if ((u16)~0 == len)
 741     return ~0;
 742
 743   return size + len;
 744 }
 745
 746 u16
 747 mac_write (u8 * p, void * a)
 748 {
 749   *(u16 *)p = clib_host_to_net_u16 (LISP_AFI_MAC);
 750   clib_memcpy(p + sizeof (u16), a, 6);
 751   return mac_size_to_write (a);
 752 }
 753
 754 u16
 755 vni_write (u8 * p, void * a)
 756 {
 757   lcaf_hdr_t _h, *h = &_h;
 758   gid_address_t * g = a;
 759   u16 size = 0, len;
 760
 761   /* put lcaf header */
 762   *(u16 *) p = clib_host_to_net_u16 (LISP_AFI_LCAF);
 763   size += sizeof (u16);
 764   memset (h, 0, sizeof (h[0]));
 765   LCAF_TYPE (h) = LCAF_INSTANCE_ID;
 766   u16 lcaf_len = sizeof (u32) /* Instance ID size */
 767     + gid_address_size_to_put_no_vni (g);
 768   LCAF_LENGTH (h) = clib_host_to_net_u16 (lcaf_len);
 769   LCAF_RES2 (h) = gid_address_vni_mask (g);
 770
 771   /* put vni header */
 772   clib_memcpy (p + size, h, sizeof (h[0]));
 773   size += sizeof (h[0]);
 774
 775   u32 * afip = (u32 *)(p + size);
 776   afip[0] = clib_host_to_net_u32 (gid_address_vni (g));
 777   size += sizeof (u32);
 778
 779   /* write the actual address */
 780   len = gid_address_put_no_vni (p + size, g);
 781
 782   if ((u16)~0 == len)
 783     return ~0;
 784
 785   return size + len;
 786 }
 787
 788 u16
 789 no_addr_write (u8 * p, void * a)
 790 {
 791   /* do nothing; return AFI field size */
 792   return sizeof (u16);
 793 }
 794
 795 u16
 796 no_addr_size_to_write (void * a)
 797 {
 798   return sizeof (u16); /* AFI field length */
 799 }
 800
 801 u16
 802 vni_size_to_write (void * a)
 803 {
 804   gid_address_t * g =  a;
 805   return (sizeof (u32) /* vni size */
 806           + sizeof (u16) /* LCAF AFI field size */
 807           + sizeof (lcaf_hdr_t)
 808     + gid_address_size_to_put_no_vni (g));
 809 }
 810
 811 u16
 812 lcaf_size_to_write (void * a)
 813 {
 814   lcaf_t * lcaf = (lcaf_t *) a;
 815   u32 size = 0, len;
 816   u8 type = lcaf_type (lcaf);
 817
 818   size += sizeof (u16); /* AFI size */
 819
 820   len = (*lcaf_size_to_write_fcts[type])(lcaf);
 821   if (~0 == len)
 822     return ~0;
 823
 824   return size + len;
 825 }
 826
 827 u16
 828 mac_size_to_write (void * a)
 829 {
 830   return sizeof (u16) + 6;
 831 }
 832
 833 u8
 834 gid_address_len (gid_address_t *a)
 835 {
 836   gid_address_type_t type = gid_address_type (a);
 837   return (*addr_len_fcts[type])((*cast_fcts[type])(a));
 838 }
 839
 840 static u16
 841 gid_address_put_no_vni (u8 * b, gid_address_t * gid)
 842 {
 843   gid_address_type_t type = gid_address_type (gid);
 844   return (*write_fcts[type])(b, (*cast_fcts[type])(gid));
 845 }
 846
 847 u16
 848 gid_address_put (u8 * b, gid_address_t * gid)
 849 {
 850   if (0 != gid_address_vni (gid))
 851     return vni_write (b, gid);
 852
 853   return gid_address_put_no_vni (b, gid);
 854 }
 855
 856 static u16
 857 gid_address_size_to_put_no_vni (gid_address_t * gid)
 858 {
 859   gid_address_type_t type = gid_address_type (gid);
 860   return (*size_to_write_fcts[type])((*cast_fcts[type])(gid));
 861 }
 862
 863 u16
 864 gid_address_size_to_put (gid_address_t * gid)
 865 {
 866   if (0 != gid_address_vni (gid))
 867     return vni_size_to_write (gid);
 868
 869   return gid_address_size_to_put_no_vni (gid);
 870 }
 871
 872 void *
 873 gid_address_cast (gid_address_t * gid, gid_address_type_t type)
 874 {
 875   return (*cast_fcts[type])(gid);
 876 }
 877
 878 void
 879 gid_address_copy(gid_address_t * dst, gid_address_t * src)
 880 {
 881   gid_address_type_t type = gid_address_type(src);
 882   (*copy_fcts[type])((*cast_fcts[type])(dst), (*cast_fcts[type])(src));
 883   gid_address_type(dst) = type;
 884   gid_address_vni(dst) = gid_address_vni(src);
 885   gid_address_vni_mask(dst) = gid_address_vni_mask(src);
 886 }
 887
 888 u32
 889 mac_parse (u8 * offset, gid_address_t * a)
 890 {
 891   /* skip AFI field */
 892   offset += sizeof (u16);
 893
 894   memcpy (gid_address_mac (a), offset, sizeof (gid_address_mac (a)));
 895   return (sizeof (u16) + sizeof (gid_address_mac (a)));
 896 }
 897
 898 u32
 899 gid_address_parse (u8 * offset, gid_address_t *a)
 900 {
 901   lisp_afi_e afi;
 902   int len = 0;
 903
 904   if (!a)
 905     return 0;
 906
 907   /* NOTE: since gid_adress_parse may be called by vni_parse, we can't 0
 908    * the gid address here */
 909   afi = clib_net_to_host_u16 (*((u16 *) offset));
 910
 911   switch (afi)
 912     {
 913     case LISP_AFI_NO_ADDR:
 914       len = sizeof(u16);
 915       gid_address_type(a) = GID_ADDR_NO_ADDRESS;
 916       break;
 917     case LISP_AFI_IP:
 918       len = ip_address_parse (offset, afi, &gid_address_ip(a));
 919       gid_address_type(a) = GID_ADDR_IP_PREFIX;
 920       /* this should be modified outside if needed*/
 921       gid_address_ippref_len(a) = 32;
 922       break;
 923     case LISP_AFI_IP6:
 924       len = ip_address_parse (offset, afi, &gid_address_ip(a));
 925       gid_address_type(a) = GID_ADDR_IP_PREFIX;
 926       /* this should be modified outside if needed*/
 927       gid_address_ippref_len(a) = 128;
 928       break;
 929     case LISP_AFI_LCAF:
 930       gid_address_type(a) = GID_ADDR_LCAF;
 931       len = lcaf_parse (offset, a);
 932       break;
 933     case LISP_AFI_MAC:
 934       len = mac_parse (offset, a);
 935       gid_address_type(a) = GID_ADDR_MAC;
 936       break;
 937     default:
 938       clib_warning("LISP AFI %d not supported!", afi);
 939       return ~0;
 940     }
 941   return len;
 942 }
 943
 944 int
 945 no_addr_cmp (void * a1, void * a2)
 946 {
 947   return 0;
 948 }
 949
 950 int
 951 vni_cmp (void * a1, void * a2)
 952 {
 953   vni_t * v1 = a1;
 954   vni_t * v2 = a2;
 955
 956   if (vni_mask_len (v1) != vni_mask_len (v2))
 957     return -1;
 958   if (vni_vni (v1) != vni_vni (v2))
 959     return -1;
 960   return gid_address_cmp (vni_gid (v1), vni_gid (v2));
 961 }
 962
 963 /* Compare two gid_address_t.
 964  * Returns:
 965  *        -1: If they are from different afi
 966  *             0: Both address are the same
 967  *             1: Addr1 is bigger than addr2
 968  *             2: Addr2 is bigger than addr1
 969  */
 970 int
 971 gid_address_cmp (gid_address_t * a1, gid_address_t * a2)
 972 {
 973   lcaf_t * lcaf1, * lcaf2;
 974   int cmp = -1;
 975   if (!a1 || !a2)
 976     return -1;
 977   if (gid_address_type(a1) != gid_address_type(a2))
 978     return -1;
 979   if (gid_address_vni(a1) != gid_address_vni(a2))
 980     return -1;
 981   if (gid_address_vni_mask(a1) != gid_address_vni_mask(a2))
 982     return -1;
 983
 984   switch (gid_address_type(a1))
 985     {
 986     case GID_ADDR_NO_ADDRESS:
 987       if (a1 == a2)
 988         cmp = 0;
 989       else
 990         cmp = 2;
 991       break;
 992     case GID_ADDR_IP_PREFIX:
 993       cmp = ip_prefix_cmp (&gid_address_ippref(a1), &gid_address_ippref(a2));
 994       break;
 995     case GID_ADDR_LCAF:
 996       lcaf1 = &gid_address_lcaf (a1);
 997       lcaf2 = &gid_address_lcaf (a2);
 998       if (lcaf_type (lcaf1) == lcaf_type (lcaf2))
 999         cmp = (*lcaf_cmp_fcts[lcaf_type (lcaf1)])(lcaf1, lcaf2);
1000       break;
1001     case GID_ADDR_MAC:
1002       cmp = memcmp (gid_address_mac (a1), gid_address_mac (a2),
1003                     sizeof (gid_address_mac (a1)));
1004       break;
1005     default:
1006       break;
1007     }
1008
1009   return cmp;
1010 }
1011
1012
1013 u32
1014 locator_parse (void * b, locator_t * loc)
1015 {
1016   locator_hdr_t * h;
1017   u8 status = 1; /* locator up */
1018   int len;
1019
1020   h = b;
1021   if (!LOC_REACHABLE(h) && LOC_LOCAL(h))
1022     status = 0;
1023
1024   len = gid_address_parse (LOC_ADDR(h), &loc->address);
1025   if (len == ~0)
1026     return len;
1027
1028   loc->state = status;
1029   loc->local = 0;
1030   loc->priority = LOC_PRIORITY(h);
1031   loc->weight = LOC_WEIGHT(h);
1032   loc->mpriority = LOC_MPRIORITY(h);
1033   loc->mweight = LOC_MWEIGHT(h);
1034
1035   return sizeof(locator_hdr_t) + len;
1036 }
1037
1038 void
1039 locator_copy (locator_t * dst, locator_t * src)
1040 {
1041   /* TODO if gid become more complex, this will need to be changed! */
1042   clib_memcpy (dst, src, sizeof(*dst));
1043   if (!src->local)
1044     gid_address_copy (&dst->address, &src->address);
1045 }
1046
1047 u32
1048 locator_cmp (locator_t * l1, locator_t * l2)
1049 {
1050   u32 ret = 0;
1051   if ((ret = gid_address_cmp (&l1->address, &l2->address)) != 0)
1052     return 1;
1053
1054   if (l1->priority != l2->priority)
1055     return 1;
1056   if (l1->weight != l2->weight)
1057     return 1;
1058   if (l1->mpriority != l2->mpriority)
1059     return 1;
1060   if (l1->mweight != l2->mweight)
1061     return 1;
1062   return 0;
1063 }
1064
1065 void
1066 locator_free (locator_t * l)
1067 {
1068   if (!l->local)
1069     gid_address_free (&l->address);
1070 }