diff options
Diffstat (limited to 'nest/a-tlv.c')
| -rw-r--r-- | nest/a-tlv.c | 421 |
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; +} |