TunnelEncaps: Initial commit

4 files changed, 942 insertions, 2 deletions

diff --git a/lib/Makefile b/lib/Makefile
index 0310eca2..1534b794 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -1,4 +1,4 @@
-src := base64.c bitmap.c bitops.c blake2s.c blake2b.c checksum.c event.c flowspec.c idm.c ip.c lists.c mac.c md5.c mempool.c net.c patmatch.c printf.c resource.c sha1.c sha256.c sha512.c slab.c slists.c strtoul.c tbf.c timer.c xmalloc.c
+src := base64.c bitmap.c bitops.c blake2s.c blake2b.c checksum.c event.c flowspec.c idm.c ip.c lists.c mac.c md5.c mempool.c net.c patmatch.c printf.c resource.c sha1.c sha256.c sha512.c slab.c slists.c strtoul.c tbf.c timer.c tunnel_encaps.c xmalloc.c
 obj := $(src-o-files)
 $(all-daemon)
 
diff --git a/lib/tunnel_encaps.c b/lib/tunnel_encaps.c
new file mode 100644
index 00000000..d549694a
--- /dev/null
+++ b/lib/tunnel_encaps.c
@@ -0,0 +1,876 @@
+#define LOCAL_DEBUG
+
+#include <stdlib.h>
+#include "lib/tunnel_encaps.h"
+
+struct format_fn {
+  node n;
+  int type;
+  const char *name;
+  format_tunnel_encap_fn cb;
+};
+
+list format_fns;
+
+static format_tunnel_encap_fn lookup_format_tunnel_encap_cb(int type);
+static const char *lookup_format_tunnel_encap_name(int type);
+static int default_format_tunnel_encap(const struct te_encap *encap, byte *buf, uint size);
+
+static
+int walk_encap(const void *p, size_t sub_tlv_len, const struct te_visitor *visitor, struct te_context *ctx)
+{
+  struct te_encap encap;
+  memset(&encap, 0, sizeof(encap));
+
+  encap.type = 0;
+  encap.length = sub_tlv_len;
+  encap.data = (void *)p;
+  return visitor->visit_encap ? visitor->visit_encap(&encap, ctx) : 0;
+}
+
+static
+int walk_color(const void *p, size_t sub_tlv_len, const struct te_visitor *visitor, struct te_context *ctx)
+{
+  if (sub_tlv_len != 8)
+  {
+    DBG(L_TRACE "WG: color len error %d", sub_tlv_len);
+    return -1;
+  }
+
+  if (get_u16(p) != 0x030b)
+  {
+    DBG(L_TRACE "WG: color error %04x", get_u16(p));
+    return -1;
+  }
+
+  return visitor->visit_color ? visitor->visit_color(get_u32(p+4), ctx) : 0;
+}
+
+static
+int walk_udp_dest_port(const void *p, size_t sub_tlv_len, const struct te_visitor *visitor, struct te_context *ctx)
+{
+  if (sub_tlv_len != 2)
+  {
+    DBG(L_TRACE "WG: udp dest port len error %d", sub_tlv_len);
+    return -1;
+  }
+
+  return visitor->visit_udp_dest_port ? visitor->visit_udp_dest_port(get_u16(p), ctx) : 0;
+}
+
+static
+int walk_tunnel_ep(const void *p, size_t sub_tlv_len, const struct te_visitor *visitor, struct te_context *ctx)
+{
+  if (sub_tlv_len < 6)
+  {
+    DBG(L_TRACE "WG: tunnel ep len error");
+    return -1;
+  }
+
+  struct te_endpoint ep;
+  memset(&ep, 0, sizeof(ep));
+
+  ep.reserved = get_u32(p);
+  u16 af = get_u16(p + 4);
+  ep.af = af;
+  switch (af)
+  {
+  case 0:
+    if (sub_tlv_len != 6)
+    {
+      DBG(L_TRACE "WG: Fam 0 len error %d", sub_tlv_len);
+      return -1;
+    }
+    ep.ip = IP6_NONE;
+    break;
+  case AFI_IPV4:
+    if (sub_tlv_len != 10)
+    {
+      DBG(L_TRACE "WG: IPv4 len error %d", sub_tlv_len);
+      return -1;
+    }
+    ep.ip = ipa_from_ip4(get_ip4(p + 6));
+    break;
+  case AFI_IPV6:
+    if (sub_tlv_len != 22)
+    {
+      DBG(L_TRACE "WG: IPv6 len error %d", sub_tlv_len);
+      return -1;
+    }
+    ep.ip = ipa_from_ip6(get_ip6(p + 6));
+    break;
+  default:
+    DBG(L_TRACE "WG: Address family error %d", af);
+    return -1;
+  }
+
+  return visitor->visit_ep ? visitor->visit_ep(&ep, ctx) : 0;
+}
+
+static
+int walk_unknown(const void *p, int type, size_t sub_tlv_len, const struct te_visitor *visitor, struct te_context *ctx)
+{
+  struct te_unknown unknown;
+  memset(&unknown, 0, sizeof(unknown));
+
+  unknown.length = sub_tlv_len;
+  unknown.data = (void *)p;
+  return visitor->visit_unknown ? visitor->visit_unknown(type, &unknown, ctx) : 0;
+}
+
+static
+int walk_sub_tlv(const u8 *p, size_t len, const struct te_visitor *visitor, struct te_context *ctx)
+{
+  if (len < 2)
+  {
+    DBG(L_TRACE "WG: sub_tlv len error %d", len);
+    return -1;
+  }
+
+  const u8 *first = p;
+  const u8 *last = p + len;
+  int type = get_u8(p++);
+  u16 sub_tlv_len = 0;
+
+  if (type >= 0 && type <= 127)
+  {
+    sub_tlv_len = get_u8(p);
+    p++;
+  }
+  else if (type >= 128 && type <= 255)
+  {
+    if (len < 3)
+    {
+      DBG(L_TRACE "WG: sub_tlv len error %d", len);
+      return -1;
+    }
+
+    sub_tlv_len = get_u16(p);
+    p += 2;
+  }
+  else
+  {
+    DBG(L_TRACE "WG: sub_tlv type error %d", type);
+    return -1;
+  }
+
+  if (p + sub_tlv_len > last)
+  {
+    DBG(L_TRACE "WG: sub_tlv value len error %d", sub_tlv_len);
+    return -1;
+  }
+
+  if (visitor->visit_subtlv && visitor->visit_subtlv(type, ctx) < 0)
+    return p - first + sub_tlv_len;
+
+  int res = 0;
+
+  switch (type)
+  {
+  case BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP:
+    res = walk_encap(p, sub_tlv_len, visitor, ctx);
+    break;
+  case BGP_TUNNEL_ENCAP_A_SUB_TLV_TUNNEL_EP:
+    res = walk_tunnel_ep(p, sub_tlv_len, visitor, ctx);
+    break;
+  case BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR:
+    res = walk_color(p, sub_tlv_len, visitor, ctx);
+    break;
+  case BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT:
+    res = walk_udp_dest_port(p, sub_tlv_len, visitor, ctx);
+    break;
+  default:
+    res = walk_unknown(p, type, sub_tlv_len, visitor, ctx);
+    break;
+  }
+
+  if (res < 0)
+  {
+    DBG(L_TRACE "WG: Decode error");
+    return res;
+  }
+
+  /* TODO use return value? */
+  if (visitor->visit_subtlv_end)
+    visitor->visit_subtlv_end(type, ctx);
+
+  return p - first + sub_tlv_len;
+}
+
+static
+int walk_sub_tlvs(const u8 *p, size_t size, const struct te_visitor *visitor, struct te_context *ctx)
+{
+  const u8 *pos = p;
+
+  while (size > 0)
+  {
+    int l = walk_sub_tlv(pos, size, visitor, ctx);
+
+    if (l < 0)
+    {
+      DBG(L_TRACE "WG: decode error %d", l);
+      return l;
+    }
+
+    ADVANCE(pos, size, l);
+  }
+
+  return pos - p;
+}
+
+static
+int walk_tlv(const u8 *p, size_t len, const struct te_visitor *visitor, struct te_context *ctx)
+{
+  const u8 *pos = p;
+  int l;
+
+  if (len < 4)
+  {
+    DBG(L_TRACE "WG: tunnel_encap len error %d", len);
+    return -1;
+  }
+
+  u16 type = get_u16(pos);
+  ADVANCE(pos, len, 2);
+
+  u16 value_length = get_u16(pos);
+  ADVANCE(pos, len, 2);
+
+  if (len < value_length)
+  {
+    DBG(L_ERR "WG: tunnel encap len error %d", value_length);
+    return -1;
+  }
+
+  ctx->sub_tlvs_pos = pos;
+  ctx->sub_tlvs_len = value_length;
+
+  if (visitor->visit_tlv && visitor->visit_tlv(type, ctx) < 0)
+    return 0;
+
+  l = walk_sub_tlvs(pos, value_length,  visitor, ctx);
+  if (l < 0)
+  {
+    DBG(L_ERR "WG: sub-TLVs decode error");
+    return l;
+  }
+
+  if (l != value_length)
+  {
+    DBG(L_ERR "WG: TLV length error");
+    return -1;
+  }
+
+  ADVANCE(pos, len, l);
+
+  if (visitor->visit_tlv_end && visitor->visit_tlv_end(ctx) < 0)
+    return 0;
+
+  ctx->sub_tlvs_pos = NULL;
+  ctx->sub_tlvs_len = 0;
+
+  return pos - p;
+}
+
+int walk_tunnel_encap(const struct adata *a, const struct te_visitor *visitor, void *opaque)
+{
+  const u8 *p = a->data;
+  const u8 *pos = p;
+  uint size = a->length;
+  struct te_context ctx = { .opaque = opaque, };
+
+  while (size > 0)
+  {
+    int l = walk_tlv(p, size, visitor, &ctx);
+
+    if (l < 0)
+    {
+      DBG(L_ERR "WG: TLV decode error");
+      return l;
+    }
+
+    ADVANCE(pos, size, l);
+  }
+
+  return 0;
+}
+
+struct count_subtlvs {
+  uint count;
+};
+
+static int
+visitor_count_subtlv(int UNUSED type, struct te_context *ctx){
+  struct count_subtlvs *data = ctx->opaque;
+
+  data->count++;
+  return 0;
+}
+
+int te_count_subtlvs(struct te_context *ctx)
+{
+  struct count_subtlvs data = { .count = 0, };
+  struct te_context new_ctx = { .opaque = &data, };
+  struct te_visitor visitor = { .visit_subtlv = visitor_count_subtlv, };
+
+  if (walk_sub_tlvs(ctx->sub_tlvs_pos, ctx->sub_tlvs_len, &visitor, &new_ctx) < 0)
+    return -1;
+
+  return data.count;
+}
+
+struct {
+  int type;
+  const char *name;
+} tunnel_types[] = {
+  {0, "Reserved"},
+  {1, "L2TPv3 over IP"},
+  {2, "GRE"},
+  {3, "Transmit tunnel endpoint (DEPRECATED)"},
+  {4, "IPsec in Tunnel-mode (DEPRECATED)"},
+  {5, "IP in IP tunnel with IPsec Transport Mode (DEPRECATED)"},
+  {6, "MPLS-in-IP tunnel with IPsec Transport Mode (DEPRECATED)"},
+  {7, "IP in IP"},
+  {8, "VXLAN Encapsulation"},
+  {9, "NVGRE Encapsulation"},
+  {10, "MPLS Encapsulation"},
+  {11, "MPLS in GRE Encapsulation"},
+  {12, "VXLAN GPE Encapsulation"},
+  {13, "MPLS in UDP Encapsulation"},
+  {14, "IPv6 Tunnel"},
+  {15, "SR TE Policy Type"},
+  {16, "Bare"},
+  {17, "SR Tunnel (DEPRECATED)"},
+  {18, "Cloud Security"},
+  {19, "Geneve Encapsulation"},
+  {20, "Any-Encapsulation"},
+  {21, "GTP Tunnel Type"},
+  {22, "Dynamic Path Selection (DPS) Tunnel Encapsulation"},
+  {23, "Originating PE (OPE)"},
+  {24, "Dynamic Path Selection (DPS) Policy"},
+};
+
+const uint num_tunnel_types = sizeof(tunnel_types)/sizeof(tunnel_types[0]);
+
+static const char *
+lookup_name(int type)
+{
+  for (uint i = 0; i < num_tunnel_types; i++)
+  {
+    if (tunnel_types[i].type == type)
+      return tunnel_types[i].name;
+  }
+
+  const char *name = lookup_format_tunnel_encap_name(type);
+  return name ? name : "Unassigned";
+}
+
+static int visitor_format_tlv(int type, struct te_context *ctx)
+{
+  struct te_buffer *buf = ctx->opaque;
+  const char *name = lookup_name(type);
+
+  int l = bsnprintf(buf->pos, buf->size, "%s{type: %s(%d)", buf->tlv?"}, ":"", name, type);
+  if (l < 0)
+  {
+    DBG(L_ERR "WG: bsnprintf error");
+    return -1;
+  }
+
+  buf->tlv++;
+  buf->tlv_type = type;
+  buf->subtlv=0;
+  ADVANCE(buf->pos, buf->size, l);
+  return 0;
+}
+
+static int visitor_format_subtlv(int UNUSED type, struct te_context *ctx)
+{
+  struct te_buffer *buf = ctx->opaque;
+
+  int l = bsnprintf(buf->pos, buf->size, ", ");
+  if (l < 0)
+    return -1;
+  ADVANCE(buf->pos, buf->size, l);
+  return 0;
+}
+
+static int visitor_format_encap(const struct te_encap *encap, struct te_context *ctx)
+{
+  struct te_buffer *buf = ctx->opaque;
+  int l = bsnprintf(buf->pos, buf->size, "encap: ");
+  if (l < 0)
+    return -1;
+  ADVANCE(buf->pos, buf->size, l);
+
+  l = lookup_format_tunnel_encap_cb(buf->tlv_type)(encap, buf->pos, buf->size);
+  if (l < 0)
+    l = default_format_tunnel_encap(encap, buf->pos, buf->size);
+
+  if (l < 0)
+  {
+    DBG(L_ERR "WG: Encapsulation format error");
+    return l;
+  }
+
+  ADVANCE(buf->pos, buf->size, l);
+  return 0;
+}
+
+static int visitor_format_ep(const struct te_endpoint *ep, struct te_context *ctx)
+{
+  struct te_buffer *buf = ctx->opaque;
+  int l =  bsnprintf(buf->pos, buf->size, "ep: %I", ep->ip);
+  if (l < 0)
+    return -1;
+  ADVANCE(buf->pos, buf->size, l);
+  return 0;
+}
+
+static int visitor_format_color(u32 color, struct te_context *ctx)
+{
+  struct te_buffer *buf = ctx->opaque;
+  int l = bsnprintf(buf->pos, buf->size, "color: %d", color);
+  if (l < 0)
+    return -1;
+  ADVANCE(buf->pos, buf->size, l);
+  return 0;
+}
+
+static int visitor_format_udp_dest_port(u16 udp_dest_port, struct te_context *ctx)
+{
+  struct te_buffer *buf = ctx->opaque;
+  int l = bsnprintf(buf->pos, buf->size, "udp: %d", udp_dest_port);
+  if (l < 0)
+    return -1;
+  ADVANCE(buf->pos, buf->size, l);
+  return 0;
+}
+
+static int visitor_format_unknown(int type, const struct te_unknown *unknown, struct te_context *ctx)
+{
+  struct te_buffer *buf = ctx->opaque;
+  const byte *data = unknown->data;
+  int l = bsnprintf(buf->pos, buf->size, "unknown(%d): ", type);
+  if (l < 0)
+    return -1;
+  ADVANCE(buf->pos, buf->size, l);
+
+  int first = 1;
+  for (uint i = 0; i < unknown->length; i++)
+  {
+    if (buf->size < 4) {
+      DBG(L_ERR "WG: Format unknown sub-TLV error");
+      return -1;
+    }
+
+    int l = bsnprintf(buf->pos, buf->size, "%s%02x", first?"":" ", data[i]);
+    ADVANCE(buf->pos, buf->size, l);
+    first = 0;
+  }
+
+  return 0;
+}
+
+struct te_visitor format_visitor =
+{
+  .visit_tlv = visitor_format_tlv,
+  .visit_subtlv = visitor_format_subtlv,
+  .visit_encap = visitor_format_encap,
+  .visit_ep = visitor_format_ep,
+  .visit_color = visitor_format_color,
+  .visit_udp_dest_port = visitor_format_udp_dest_port,
+  .visit_unknown = visitor_format_unknown,
+};
+
+const struct te_visitor *te_get_format_visitor(void)
+{
+  return &format_visitor;
+}
+
+int format_tunnel_encap(const struct adata *a, byte *buf, uint size)
+{
+  struct te_buffer buffer = {.pos=buf, .size=size, .tlv=0, .subtlv=0};
+  int l = bsnprintf(buffer.pos, buffer.size, "[");
+  if (l < 0)
+    return -1;
+  ADVANCE(buffer.pos, buffer.size, l);
+
+  walk_tunnel_encap(a, &format_visitor, &buffer);
+
+  l = bsnprintf(buffer.pos, buffer.size, "%s]", buffer.tlv?"}":"");
+  if (l < 0)
+    return -1;
+  ADVANCE(buffer.pos, buffer.size, l);
+  return buffer.pos - buf;
+}
+
+static int default_format_tunnel_encap(const struct te_encap *encap, byte *buf, uint size)
+{
+  byte *pos = buf;
+  const byte *data = encap->data;
+  int first = 1;
+
+  for (uint i = 0; i < encap->length; i++)
+  {
+    if (size < 4) {
+      DBG(L_ERR "WG: Default format tunnel encap error");
+      return pos - buf;
+    }
+
+    int l = bsnprintf(pos, size, "%s%02x", first?"":" ", data[i]);
+    ADVANCE(pos, size, l);
+
+    first = 0;
+  }
+
+  return pos - buf;
+}
+
+static struct format_fn *lookup_format_tunnel_encap(int type)
+{
+  struct format_fn *n;
+  WALK_LIST(n, format_fns) {
+    if (n->type == type)
+      return n;
+  }
+
+  return NULL;
+}
+
+static const char *lookup_format_tunnel_encap_name(int type)
+{
+  struct format_fn *fn = lookup_format_tunnel_encap(type);
+
+  if (fn && fn->name)
+    return fn->name;
+
+  return NULL;
+}
+
+static format_tunnel_encap_fn lookup_format_tunnel_encap_cb(int type)
+{
+  struct format_fn *fn = lookup_format_tunnel_encap(type);;
+
+  if (fn && fn->cb)
+    return fn->cb;
+
+  DBG("lookup_format_tunnel_encap not found: %d\n", type);
+  return default_format_tunnel_encap;
+}
+
+struct tlv_buffer {
+  uint len;
+  uint size;
+  byte *p;
+  byte *tlv_len_p;
+  byte *subtlv_len_p;
+};
+
+static int calc_tlv(int type, struct te_context *ctx)
+{
+  struct tlv_buffer *buf = ctx->opaque;
+  /*
+      Header:
+      Tunnel Type (2 Octets)
+      Length (2 Octets)
+      Value (Variable)
+  */
+  buf->len += 4;
+
+  if (!buf->size)
+    return 0;
+
+  if (buf->len > buf->size)
+    return -1;
+
+  if (type <= 0 || type > 65535)
+    return -1;
+
+  put_u16(buf->p, type);
+  buf->p += 2;
+
+  buf->tlv_len_p = buf->p;
+
+  put_u16(buf->p, 0xcccc);		/* Update in calc_tlv_end */
+  buf->p += 2;
+  return 0;
+}
+
+static int calc_tlv_end(struct te_context *ctx)
+{
+  struct tlv_buffer *buf = ctx->opaque;
+
+  if (!buf->size)
+    return 0;
+
+  if (!buf->tlv_len_p)
+    return -1;
+
+  put_u16(buf->tlv_len_p, buf->p - buf->tlv_len_p - 2);
+  buf->tlv_len_p = NULL;
+  return 0;
+}
+
+static int calc_subtlv(int type, struct te_context *ctx)
+{
+  struct tlv_buffer *buf = ctx->opaque;
+  /*
+    Sub-TLV Type (1 Octet)
+    Sub-TLV Length (1 or 2 Octets)
+  */
+  if (type >= 0 && type <= 127)
+    buf->len += 2;
+  else if (type >= 128 && type <= 255)
+    buf->len += 3;
+  else
+  {
+    DBG(L_ERR "calc_subtlv bad type %d\n", type );
+    return -1;
+  }
+
+  if (!buf->size)
+    return 0;
+
+  if (buf->len > buf->size)
+    return -1;
+
+  put_u8(buf->p, type);
+  buf->p++;
+
+  buf->subtlv_len_p = buf->p;
+
+  if (type >= 0 && type <= 127)
+  {
+    put_u8(buf->p, 0xdd);	/* Update in calc_subtlv_end */
+    buf->p++;
+  }
+  else
+  {
+    put_u16(buf->p, 0xeeee);	/* Update in calc_subtlv_end */
+    buf->p += 2;
+  }
+
+  return 0;
+}
+
+static int calc_subtlv_end(int type, struct te_context *ctx)
+{
+  struct tlv_buffer *buf = ctx->opaque;
+
+  if (!buf->size)
+    return 0;
+
+  if (!buf->subtlv_len_p)
+    return -1;
+
+  if (type >= 0 && type <= 127)
+  {
+    put_u8(buf->subtlv_len_p, buf->p - buf->subtlv_len_p - 1);
+    buf->subtlv_len_p = NULL;
+  }
+  else if (type >= 128 && type <= 255)
+  {
+    put_u16(buf->subtlv_len_p, buf->p - buf->subtlv_len_p - 2);
+    buf->subtlv_len_p = NULL;
+  }
+  else
+    return -1;
+
+  return 0;
+}
+
+static int calc_encap(const struct te_encap *encap, struct te_context *ctx)
+{
+  struct tlv_buffer *buf = ctx->opaque;
+  /*
+    Sub-TLV Value (Variable)
+   */
+  if (encap->type <= 0)
+  {
+    DBG(L_ERR "calc_encap bad type %d\n", encap->type );
+    return -1;
+  }
+
+  buf->len += encap->length;
+
+  if (!buf->size)
+    return 0;
+
+  if (buf->len > buf->size)
+    return -1;
+
+  memcpy(buf->p, encap->data, encap->length);
+  buf->p += encap->length;
+  return 0;
+}
+
+static int calc_ep(const struct te_endpoint *ep, struct te_context *ctx)
+{
+  struct tlv_buffer *buf = ctx->opaque;
+  /*
+    Sub-TLV Value (10 or 22 Octets)
+  */
+  if (ep->af == AFI_IPV4 && ipa_is_ip4(ep->ip))
+    buf->len += 10;
+  else if (ep->af == AFI_IPV6 && ipa_is_ip6(ep->ip))
+    buf->len += 22;
+  else if (ep->af == 0)
+    buf->len += 6;
+  else
+  {
+    DBG(L_ERR "calc_encap bad af %04x\n", ep->af );
+    return -1;
+  }
+
+  if (!buf->size)
+    return 0;
+
+  if (buf->len > buf->size)
+    return -1;
+
+  put_u32(buf->p, ep->reserved);
+  buf->p += 4;
+
+  put_u16(buf->p, ep->af);
+  buf->p += 2;
+
+  switch (ep->af)
+  {
+  case AFI_IPV4:
+    put_ip4(buf->p, ipa_to_ip4(ep->ip));
+    buf->p += 4;
+    break;
+  case AFI_IPV6:
+    put_ip6(buf->p, ipa_to_ip6(ep->ip));
+    buf->p += 16;
+    break;
+  default:
+    break;
+  }
+
+  return 0;
+}
+
+static int calc_color(u32 color, struct te_context *ctx)
+{
+  struct tlv_buffer *buf = ctx->opaque;
+  /*
+    Sub-TLV Value (8 Octets)
+  */
+  buf->len += 8;
+
+  if (!buf->size)
+    return 0;
+
+  if (buf->len > buf->size)
+    return -1;
+
+  put_u32(buf->p, 0x030b0000);
+  buf->p += 4;
+
+  put_u32(buf->p, color);
+  buf->p += 4;
+  return 0;
+}
+
+static int calc_udp_dest_port(u16 UNUSED udp_dest_port, struct te_context *ctx)
+{
+  struct tlv_buffer *buf = ctx->opaque;
+  /*
+    Sub-TLV Value (2 Octets)
+  */
+  buf->len += 2;
+
+  if (!buf->size)
+    return 0;
+
+  if (buf->len > buf->size)
+    return -1;
+
+  put_u16(buf->p, udp_dest_port);
+  buf->p += 2;
+  return 0;
+}
+
+static int calc_unknown(int UNUSED type, const struct te_unknown *unknown, struct te_context *ctx)
+{
+  struct tlv_buffer *buf = ctx->opaque;
+  /*
+    Sub-TLV Value (Variable)
+   */
+
+  buf->len += unknown->length;
+
+  if (!buf->size)
+    return 0;
+
+  if (buf->len > buf->size)
+    return -1;
+
+  memcpy(buf->p, unknown->data, unknown->length);
+  buf->p += unknown->length;
+  return 0;
+}
+
+struct te_visitor encode_visitor =
+{
+  .visit_tlv = calc_tlv,
+  .visit_tlv_end = calc_tlv_end,
+  .visit_subtlv = calc_subtlv,
+  .visit_subtlv_end = calc_subtlv_end,
+  .visit_encap = calc_encap,
+  .visit_ep = calc_ep,
+  .visit_color = calc_color,
+  .visit_udp_dest_port = calc_udp_dest_port,
+  .visit_unknown = calc_unknown,
+};
+
+const struct te_visitor *te_get_encode_visitor(void)
+{
+  return &encode_visitor;
+}
+
+int te_init_encode_visitor(struct tlv_buffer *buf, byte *p, uint size)
+{
+  if (!buf)
+    return sizeof(struct te_buffer);
+
+  memset(buf, 0, sizeof(struct te_buffer));
+  buf->size = size;
+  buf->p = p;
+
+  return 0;
+}
+
+uint te_get_encode_length(struct tlv_buffer *buf)
+{
+  return buf->len;
+}
+
+void register_format_tunnel_encap(int type, const char *name, format_tunnel_encap_fn cb)
+{
+  struct format_fn *fn = malloc(sizeof(struct format_fn));
+  memset(fn, 0, sizeof(*fn));
+  fn->type = type;
+  fn->name = name;
+  fn->cb = cb;
+  add_head(&format_fns, &fn->n);
+}
+
+void unregister_format_tunnel_encap(int type, format_tunnel_encap_fn cb)
+{
+  struct format_fn *n, *nxt;
+  WALK_LIST_DELSAFE(n, nxt, format_fns)
+  {
+    if (n->type == type && n->cb == cb)
+    {
+      rem_node(&n->n);
+    }
+  }
+}
+
+void tunnel_encap_init()
+{
+  init_list(&format_fns);
+}
diff --git a/lib/tunnel_encaps.h b/lib/tunnel_encaps.h
new file mode 100644
index 00000000..fc7bbda6
--- /dev/null
+++ b/lib/tunnel_encaps.h
@@ -0,0 +1,63 @@
+#ifndef _BIRD_TUNNEL_ENCAPS_
+#define _BIRD_TUNNEL_ENCAPS_
+
+#include "nest/attrs.h"
+#include "nest/route.h"
+
+#define BA_TUNNEL_ENCAP		0x17
+
+#define BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP         1
+#define BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR         4
+#define BGP_TUNNEL_ENCAP_A_SUB_TLV_TUNNEL_EP     6
+#define BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT 8
+
+#define FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP         (1<<BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP)
+#define FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR         (1<<BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR)
+#define FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_TUNNEL_EP     (1<<BGP_TUNNEL_ENCAP_A_SUB_TLV_TUNNEL_EP)
+#define FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT (1<<BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT)
+
+struct te_context {
+  void *opaque;
+  const byte *sub_tlvs_pos;
+  uint sub_tlvs_len;
+};
+
+struct te_visitor {
+  int (*visit_tlv)(int type, struct te_context *ctx);
+  int (*visit_tlv_end)(struct te_context *ctx);
+  int (*visit_subtlv)(int type, struct te_context *ctx);
+  int (*visit_subtlv_end)(int type, struct te_context *ctx);
+  int (*visit_encap)(const struct te_encap *encap, struct te_context *ctx);
+  int (*visit_ep)(const struct te_endpoint *ep, struct te_context *ctx);
+  int (*visit_color)(u32 color, struct te_context *ctx);
+  int (*visit_udp_dest_port)(u16 port, struct te_context *ctx);
+  int (*visit_unknown)(int type, const struct te_unknown *unknown, struct te_context *ctx);
+};
+
+struct te_buffer {
+  byte *pos;
+  uint size;
+  int tlv;
+  int tlv_type;
+  int subtlv;
+};
+
+struct tlv_buffer;
+
+typedef int (*format_tunnel_encap_fn)(const struct te_encap *, byte *buf, uint size);
+
+void tunnel_encap_init(void);
+int walk_tunnel_encap(const struct adata *a, const struct te_visitor *visitor, void *opaque);
+int te_count_subtlvs(struct te_context *ctx);
+
+const struct te_visitor *te_get_format_visitor(void);
+int format_tunnel_encap(const struct adata *a, byte *buf, uint size);
+
+const struct te_visitor *te_get_encode_visitor(void);
+int te_init_encode_visitor(struct tlv_buffer *buf, byte *p, uint size);
+uint te_get_encode_length(struct tlv_buffer *buf);
+
+void register_format_tunnel_encap(int type, const char *name, format_tunnel_encap_fn cb);
+void unregister_format_tunnel_encap(int type, format_tunnel_encap_fn cb);
+
+#endif	/* _BIRD_TUNNEL_ENCAPS_ */
diff --git a/lib/unaligned.h b/lib/unaligned.h
index dfe0906f..0d3c83f9 100644
--- a/lib/unaligned.h
+++ b/lib/unaligned.h
@@ -17,8 +17,9 @@
  *  if possible.
  */
 
+#include <memory.h>
+#include "sysdep/config.h"
 #include "sysdep/unix/endian.h"
-#include "lib/string.h"
 
 static inline u8
 get_u8(const void *p)