TunnelEncaps: Initial commit

1 files changed, 421 insertions, 0 deletions

diff --git a/nest/a-tlv.c b/nest/a-tlv.c
new file mode 100644
index 00000000..13e05f43
--- /dev/null
+++ b/nest/a-tlv.c
@@ -0,0 +1,421 @@
+#include "lib/tunnel_encaps.h"
+#include "filter/data.h"
+
+static int
+walk_tlvlist(const struct te_tlvlist *set, const struct te_visitor *visitor, void *opaque)
+{
+  if (!set)
+    return 0;
+
+  struct te_tlv *t;
+  struct te_context ctx = { .opaque = opaque, };
+
+  WALK_LIST(t, set->tlv)
+  {
+    if (visitor->visit_tlv && visitor->visit_tlv(t->type, &ctx) < 0)
+      continue;
+
+    for (uint j=0; j < t->len; j++)
+    {
+      const struct te_subtlv *st = &t->st[j];
+
+      if (visitor->visit_subtlv && visitor->visit_subtlv(st->type, &ctx) < 0)
+        continue;
+
+      switch (st->type)
+      {
+      case TLV_ENCAPSULATION:
+        if (visitor->visit_encap && visitor->visit_encap(&st->u.tunnel_encap, &ctx) < 0)
+          return -1;
+        break;
+      case TLV_COLOR:
+        if (visitor->visit_color && visitor->visit_color(st->u.color, &ctx) < 0)
+          return -1;
+        break;
+      case TLV_TUNNEL_ENDPOINT:
+        if (visitor->visit_ep && visitor->visit_ep(&st->u.tunnel_endpoint, &ctx) < 0)
+          return -1;
+        break;
+      case TLV_UDP_DEST_PORT:
+        if (visitor->visit_udp_dest_port && visitor->visit_udp_dest_port(st->u.udp_dest_port, &ctx) < 0)
+          return -1;
+        break;
+      default:
+        if (visitor->visit_unknown && visitor->visit_unknown(st->type, &st->u.unknown, &ctx) < 0)
+          return -1;
+        break;
+      }
+
+      if (visitor->visit_subtlv_end)
+        visitor->visit_subtlv_end(st->type, &ctx); /* TODO use return value? */
+    }
+
+    if (visitor->visit_tlv_end)
+      visitor->visit_tlv_end(&ctx); /* TODO use return value? */
+  }
+
+  return 0;
+}
+
+static void calc_elems(const struct f_tree UNUSED *t, void *ptr)
+{
+  uint *len = ptr;
+  (*len)++;
+}
+
+static void put_elems(const struct f_tree *t, void *ptr)
+{
+  struct te_tlv *tlv = ptr;
+  const struct te_subtlv *st = &t->from.val.st;
+  tlv->len--;
+  tlv->st[tlv->len] = *st;
+  if (st->type == TLV_ENCAPSULATION)
+    tlv->type = st->u.tunnel_encap.type;
+  DBG("put_elems: type %d\n", st->type);
+}
+
+const struct te_tlv *tlv_alloc(struct linpool *pool, const struct f_tree *set)
+{
+  uint len = 0;
+
+  tree_walk(set, calc_elems, &len);
+
+  if (!len)
+    return NULL;
+
+  struct te_tlv *res = lp_alloc(pool, sizeof(struct te_tlv) + len * sizeof(struct te_subtlv));
+
+  res->len = len;
+  tree_walk(set, put_elems, res);
+
+  res->len = len;		/* Len was reset during tree_walk. */
+
+  for (uint i=0; i < len; i++)
+  {
+    DBG("tlv_alloc: type[%d]: %d\n", i, res->st[i].type);
+  }
+  return res;
+}
+
+int tlv_set_format(const struct te_tlvlist *set, int UNUSED from, byte *buf, uint size)
+{
+  const struct te_visitor *visitor = te_get_format_visitor();
+  struct te_buffer buffer = {.pos=buf, .size=size, .tlv=0, .subtlv=0};
+
+  if (set)
+    walk_tlvlist(set, visitor, &buffer);
+
+  int l = bsnprintf(buffer.pos, buffer.size, "%s", buffer.tlv?"}":"<empty>");
+  if (l < 0)
+    return -1;
+  ADVANCE(buffer.pos, buffer.size, l);
+  return buffer.pos - buf;
+}
+
+int
+tlv_set_contains(const struct te_tlvlist *list, const struct te_tlv UNUSED *val)
+{
+  if (!list)
+    return 0;
+
+  /* FIXME */
+  return 0;
+}
+
+static void tlv_add_copy(struct linpool *pool, struct te_tlvlist *list, const struct te_tlv *t)
+{
+  uint size = sizeof(struct te_tlv) + t->len * sizeof(struct te_subtlv);
+  struct te_tlv *tlv = lp_alloc(pool, size);
+  memcpy(tlv, t, size);
+  memset(&tlv->n, 0, sizeof(tlv->n));
+  add_tail(&list->tlv, &tlv->n);
+  DBG("tlv_add_copy: src type %d, dst type %d\n", t->st[0].type, tlv->st[0].type);
+}
+
+const struct te_tlvlist *
+tlv_set_add(struct linpool *pool, const struct te_tlvlist *list, const struct te_tlv *val)
+{
+  if (tlv_set_contains(list, val))
+    return list;
+
+  struct te_tlvlist *res = lp_alloc(pool, sizeof(struct te_tlvlist));
+
+  init_list(&res->tlv);
+
+  DBG("tlv_set_add: first type %d, second type %d\n", list?((const struct te_tlv *)HEAD(list->tlv))->st[0].type:-1, val->st[0].type);
+
+  if (list)
+  {
+    struct te_tlv *n;
+    WALK_LIST(n, list->tlv)
+    {
+      tlv_add_copy(pool, res, n);
+    }
+  }
+
+  tlv_add_copy(pool, res, val);
+  return res;
+}
+
+const struct te_tlvlist *
+tlv_set_union(struct linpool UNUSED *pool, const struct te_tlvlist *l1, const struct te_tlvlist *l2)
+{
+  if (!l1)
+    return l2;
+  if (!l2)
+    return l1;
+
+  /* FIXME */
+  void *res = NULL;
+  return res;
+}
+
+static int
+subtlv_same(const struct te_subtlv *st1, const struct te_subtlv *st2)
+{
+  if (st1 == NULL && st2 == NULL)
+    return 1;
+
+  if (st1 == NULL || st2 == NULL)
+    return 0;
+
+  if (st1->type != st2->type)
+    return 0;
+
+  switch(st1->type)
+  {
+  case TLV_ENCAPSULATION:
+    if (st1->u.tunnel_encap.type != st2->u.tunnel_encap.type ||
+	st1->u.tunnel_encap.length != st2->u.tunnel_encap.length ||
+	memcmp(st1->u.tunnel_encap.data, st2->u.tunnel_encap.data, st1->u.tunnel_encap.length))
+      return 0;
+    break;
+  case TLV_COLOR:
+    if (st1->u.color != st2->u.color)
+      return 0;
+    break;
+  case TLV_TUNNEL_ENDPOINT:
+    if (st1->u.tunnel_endpoint.reserved != st2->u.tunnel_endpoint.reserved ||
+	st1->u.tunnel_endpoint.af != st2->u.tunnel_endpoint.af ||
+	st1->u.tunnel_endpoint.af && ipa_compare(st1->u.tunnel_endpoint.ip, st2->u.tunnel_endpoint.ip))
+      return 0;
+    break;
+  case TLV_UDP_DEST_PORT:
+    if (st1->u.udp_dest_port != st2->u.udp_dest_port)
+      return 0;
+    break;
+  default:
+    if (st1->u.unknown.length != st2->u.unknown.length ||
+	memcmp(st1->u.unknown.data, st2->u.unknown.data, st1->u.unknown.length))
+      return 0;
+    break;
+  }
+
+  return 1;
+}
+
+static int
+tlv_same(const struct te_tlv *t1, const struct te_tlv *t2)
+{
+  if (t1 == NULL && t2 == NULL)
+    return 1;
+
+  if (t1 == NULL || t2 == NULL)
+    return 0;
+
+  if (t1->type != t2->type ||
+      t1->len != t2->len)
+    return 0;
+
+  for (uint i=0; i < t1->len; i++)
+    {
+      if (!subtlv_same(&t1->st[i], &t2->st[i]))
+	return 0;
+    }
+
+  return 1;
+}
+
+int
+tlvlist_same(const struct te_tlvlist *tl1, const struct te_tlvlist *tl2)
+{
+  if (tl1 == NULL && tl2 == NULL)
+    return 1;
+
+  if (tl1 == NULL || tl2 == NULL)
+    return 0;
+
+  const struct te_tlv *t1 = HEAD(tl1->tlv);
+  const struct te_tlv *t2 = HEAD(tl2->tlv);
+
+  while (NODE_VALID(t1) || NODE_VALID(t2))
+  {
+    if (!NODE_VALID(t1) || !NODE_VALID(t2))
+      return 0;
+
+    /* Both are valid. */
+    if (!tlv_same(t1, t2))
+      return 0;
+  }
+
+  return 1;
+}
+
+static int
+tlvlist_calc_tunnel_encap_new(const struct te_tlvlist *t, struct adata *ad)
+{
+  if (t == NULL)
+    return 0;
+
+  uint size = ad ? ad->length : 0;
+  byte *p = ad ? ad->data : NULL;
+
+  uint buf_size = te_init_encode_visitor(NULL, NULL, 0);
+  struct tlv_buffer *buf = alloca(buf_size);
+
+  if (te_init_encode_visitor(buf, p, size) < 0)
+    return -1;
+
+  if (walk_tlvlist(t, te_get_encode_visitor(), buf) < 0)
+    return -1;
+
+  return te_get_encode_length(buf);
+}
+
+struct adata *
+tlvlist_encode_tunnel_encap(struct linpool *pool, const struct te_tlvlist *tl)
+{
+  int len = tlvlist_calc_tunnel_encap_new(tl, NULL);
+
+  if (len < 0)
+    return NULL;
+
+  struct adata *ad = lp_alloc(pool, sizeof(struct adata) + len);
+  ad->length = len;
+
+  if (tlvlist_calc_tunnel_encap_new(tl, ad) < 0)
+    return NULL;
+
+  return ad;
+}
+
+struct decoder_buf
+{
+  struct linpool *pool;
+  struct te_tlvlist *tl;
+  struct te_tlv *cur_tlv;
+  struct te_subtlv *cur_subtlv;
+  int cur_subtlv_index;
+};
+
+static int visitor_decode_tlv(int type, struct te_context *ctx)
+{
+  struct decoder_buf *buf = ctx->opaque;
+
+  int count = te_count_subtlvs(ctx);
+
+  if (count < 0)
+    return -1;
+
+  buf->cur_tlv = lp_allocz(buf->pool, sizeof(struct te_tlv) + count * sizeof(struct te_subtlv));
+  buf->cur_tlv->type = type;
+  return 0;
+}
+
+static int visitor_decode_tlv_end(struct te_context *ctx)
+{
+  struct decoder_buf *buf = ctx->opaque;
+
+  add_tail(&buf->tl->tlv, &buf->cur_tlv->n);
+  buf->cur_tlv->len = buf->cur_subtlv_index;
+  buf->cur_tlv = NULL;
+  return 0;
+}
+
+static int
+visitor_decode_subtlv(int type, struct te_context *ctx){
+  struct decoder_buf *buf = ctx->opaque;
+
+  buf->cur_subtlv = &buf->cur_tlv->st[buf->cur_subtlv_index];
+  buf->cur_subtlv->type = type;
+  return 0;
+}
+
+static int
+visitor_decode_subtlv_end(int UNUSED type, struct te_context *ctx){
+  struct decoder_buf *buf = ctx->opaque;
+
+  buf->cur_subtlv_index++;
+  return 0;
+}
+
+static int
+visitor_decode_encap(const struct te_encap *encap, struct te_context *ctx)
+{
+  struct decoder_buf *buf = ctx->opaque;
+
+  buf->cur_subtlv->u.tunnel_encap = *encap;
+  return 0;
+}
+
+static int
+visitor_decode_ep(const struct te_endpoint *ep, struct te_context *ctx)
+{
+  struct decoder_buf *buf = ctx->opaque;
+
+  buf->cur_subtlv->u.tunnel_endpoint = *ep;
+  return 0;
+}
+
+static int
+visitor_decode_color(u32 color, struct te_context *ctx)
+{
+  struct decoder_buf *buf = ctx->opaque;
+
+  buf->cur_subtlv->u.color = color;
+  return 0;
+}
+
+static int
+visitor_decode_udp_dest_port(u16 port, struct te_context *ctx)
+{
+  struct decoder_buf *buf = ctx->opaque;
+
+  buf->cur_subtlv->u.udp_dest_port = port;
+  return 0;
+}
+
+static int
+visitor_decode_unknown(int UNUSED type, const struct te_unknown *unknown, struct te_context *ctx)
+{
+  struct decoder_buf *buf = ctx->opaque;
+
+  buf->cur_subtlv->u.unknown = *unknown;
+  return 0;
+}
+
+struct te_tlvlist *tlvlist_decode_tunnel_encap(struct linpool *pool,
+					       const struct adata *ad)
+{
+  struct te_tlvlist *tl = lp_alloc(pool, sizeof(struct te_tlvlist));
+  struct decoder_buf buf = { .pool = pool, .tl = tl, };
+  struct te_visitor visitor = {
+    .visit_tlv = visitor_decode_tlv,
+    .visit_tlv_end = visitor_decode_tlv_end,
+    .visit_subtlv = visitor_decode_subtlv,
+    .visit_subtlv_end = visitor_decode_subtlv_end,
+    .visit_encap = visitor_decode_encap,
+    .visit_ep = visitor_decode_ep,
+    .visit_color = visitor_decode_color,
+    .visit_udp_dest_port = visitor_decode_udp_dest_port,
+    .visit_unknown = visitor_decode_unknown,
+  };
+
+  init_list(&tl->tlv);
+
+  int res = walk_tunnel_encap(ad, &visitor, &buf);
+  if (res < 0)
+    return NULL;
+
+  return tl;
+}