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