open5gs/lib/core/ogs-tlv-msg.c

/*
 * Copyright (C) 2019 by Sukchan Lee <acetcom@gmail.com>
 * Copyright (C) 2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
 *
 * This file is part of Open5GS.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

#include "ogs-core.h"

ogs_tlv_desc_t ogs_tlv_desc_more1 = {
    OGS_TLV_MORE, "More", 0, 1, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more2 = {
    OGS_TLV_MORE, "More", 0, 2, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more3 = {
    OGS_TLV_MORE, "More", 0, 3, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more4 = {
    OGS_TLV_MORE, "More", 0, 4, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more5 = {
    OGS_TLV_MORE, "More", 0, 5, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more6 = {
    OGS_TLV_MORE, "More", 0, 6, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more7 = {
    OGS_TLV_MORE, "More", 0, 7, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more8 = {
    OGS_TLV_MORE, "More", 0, 8, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more9 = {
    OGS_TLV_MORE, "More", 0, 9, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more10 = {
    OGS_TLV_MORE, "More", 0, 10, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more11 = {
    OGS_TLV_MORE, "More", 0, 11, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more12 = {
    OGS_TLV_MORE, "More", 0, 12, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more13 = {
    OGS_TLV_MORE, "More", 0, 13, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more14 = {
    OGS_TLV_MORE, "More", 0, 14, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more15 = {
    OGS_TLV_MORE, "More", 0, 15, 0, 0, { NULL } };
ogs_tlv_desc_t ogs_tlv_desc_more16 = {
    OGS_TLV_MORE, "More", 0, 16, 0, 0, { NULL } };

/* Return specific TLV mode based on its TLV description type and the msg
 * provided mode (used to know the type length) */
static uint8_t tlv_ctype2mode(ogs_tlv_type_e ctype, uint8_t msg_mode)
{
    switch(ctype) {
    case OGS_TLV_UINT8:
    case OGS_TLV_UINT16:
    case OGS_TLV_UINT24:
    case OGS_TLV_UINT32:
    case OGS_TLV_INT8:
    case OGS_TLV_INT16:
    case OGS_TLV_INT24:
    case OGS_TLV_INT32:
    case OGS_TLV_FIXED_STR:
    case OGS_TLV_VAR_STR:
    case OGS_TLV_NULL:
    case OGS_TLV_MORE:
    case OGS_TLV_COMPOUND:
    case OGS_TLV_MESSAGE:
        return msg_mode;
    case OGS_TV_UINT8:
    case OGS_TV_UINT16:
    case OGS_TV_UINT24:
    case OGS_TV_UINT32:
    case OGS_TV_INT8:
    case OGS_TV_INT16:
    case OGS_TV_INT24:
    case OGS_TV_INT32:
    case OGS_TV_FIXED_STR:
    case OGS_TV_VAR_STR:
    case OGS_TV_NULL:
    case OGS_TV_MORE:
        if (msg_mode == OGS_TLV_MODE_T2_L2)
            return OGS_TLV_MODE_T1; /* Here return OGS_TLV_MODE_T2 if ever existing */
        else
            return OGS_TLV_MODE_T1;
    default:
        ogs_assert_if_reached();
        break;
    }
}

static ogs_tlv_t *tlv_add_leaf(
        ogs_tlv_t *parent_tlv, ogs_tlv_t *tlv, ogs_tlv_desc_t *desc, void *msg, uint8_t msg_mode)
{
    uint8_t tlv_mode = tlv_ctype2mode(desc->ctype, msg_mode);

    switch (desc->ctype) {
    case OGS_TLV_UINT8:
    case OGS_TLV_INT8:
    case OGS_TV_UINT8:
    case OGS_TV_INT8:
    {
        ogs_tlv_uint8_t *v = (ogs_tlv_uint8_t *)msg;
        if (parent_tlv) {
            tlv = ogs_tlv_embed(parent_tlv, tlv_mode,
                    desc->type, 1, desc->instance, &v->u8);
            if (!tlv) {
                ogs_error("ogs_tlv_embed()");
                return NULL;
            }
        } else {
            tlv = ogs_tlv_add(tlv, tlv_mode,
                    desc->type, 1, desc->instance, &v->u8);
            if (!tlv) {
                ogs_error("ogs_tlv_add()");
                return NULL;
            }
        }
        break;
    }
    case OGS_TLV_UINT16:
    case OGS_TLV_INT16:
    case OGS_TV_UINT16:
    case OGS_TV_INT16:
    {
        ogs_tlv_uint16_t *v = (ogs_tlv_uint16_t *)msg;

        v->u16 = htobe16(v->u16);

        if (parent_tlv) {
            tlv = ogs_tlv_embed(parent_tlv, tlv_mode,
                    desc->type, 2, desc->instance, &v->u16);
            if (!tlv) {
                ogs_error("ogs_tlv_embed()");
                return NULL;
            }
        } else {
            tlv = ogs_tlv_add(tlv, tlv_mode,
                    desc->type, 2, desc->instance, &v->u16);
            if (!tlv) {
                ogs_error("ogs_tlv_add()");
                return NULL;
            }
        }
        break;
    }
    case OGS_TLV_UINT24:
    case OGS_TLV_INT24:
    case OGS_TV_UINT24:
    case OGS_TV_INT24:
    {
        ogs_tlv_uint24_t *v = (ogs_tlv_uint24_t *)msg;

        v->u24 = v->u24 << 8;
        v->u24 = htobe32(v->u24);

        if (parent_tlv) {
            tlv = ogs_tlv_embed(parent_tlv, tlv_mode,
                    desc->type, 3, desc->instance, &v->u24);
            if (!tlv) {
                ogs_error("ogs_tlv_embed()");
                return NULL;
            }
        } else {
            tlv = ogs_tlv_add(tlv, tlv_mode,
                    desc->type, 3, desc->instance, &v->u24);
            if (!tlv) {
                ogs_error("ogs_tlv_add()");
                return NULL;
            }
        }
        break;
    }
    case OGS_TLV_UINT32:
    case OGS_TLV_INT32:
    case OGS_TV_UINT32:
    case OGS_TV_INT32:
    {
        ogs_tlv_uint32_t *v = (ogs_tlv_uint32_t *)msg;

        v->u32 = htobe32(v->u32);

        if (parent_tlv) {
            tlv = ogs_tlv_embed(parent_tlv, tlv_mode,
                    desc->type, 4, desc->instance, &v->u32);
            if (!tlv) {
                ogs_error("ogs_tlv_embed()");
                return NULL;
            }
        } else {
            tlv = ogs_tlv_add(tlv, tlv_mode,
                    desc->type, 4, desc->instance, &v->u32);
            if (!tlv) {
                ogs_error("ogs_tlv_add()");
                return NULL;
            }
        }
        break;
    }
    case OGS_TLV_FIXED_STR:
    case OGS_TV_FIXED_STR:
    {
        ogs_tlv_octet_t *v = (ogs_tlv_octet_t *)msg;

        if (parent_tlv) {
            tlv = ogs_tlv_embed(parent_tlv, tlv_mode,
                    desc->type, desc->length, desc->instance, v->data);
            if (!tlv) {
                ogs_error("ogs_tlv_embed()");
                return NULL;
            }
        } else {
            tlv = ogs_tlv_add(tlv, tlv_mode,
                    desc->type, desc->length, desc->instance, v->data);
            if (!tlv) {
                ogs_error("ogs_tlv_add()");
                return NULL;
            }
        }
        break;
    }
    case OGS_TLV_VAR_STR:
    {
        ogs_tlv_octet_t *v = (ogs_tlv_octet_t *)msg;

        if (v->len == 0) {
            ogs_error("No TLV length - [%s] T:%d I:%d (vsz=%d)",
                    desc->name, desc->type, desc->instance, desc->vsize);
            return NULL;
        }

        if (parent_tlv) {
            tlv = ogs_tlv_embed(parent_tlv, tlv_mode,
                    desc->type, v->len, desc->instance, v->data);
            if (!tlv) {
                ogs_error("ogs_tlv_embed()");
                return NULL;
            }
        } else {
            tlv = ogs_tlv_add(tlv, tlv_mode,
                    desc->type, v->len, desc->instance, v->data);
            if (!tlv) {
                ogs_error("ogs_tlv_add()");
                return NULL;
            }
        }
        break;
    }
    case OGS_TLV_NULL:
    case OGS_TV_NULL:
    {
        if (parent_tlv) {
            tlv = ogs_tlv_embed(parent_tlv, tlv_mode,
                    desc->type, 0, desc->instance, NULL);
            if (!tlv) {
                ogs_error("ogs_tlv_embed()");
                return NULL;
            }
        } else {
            tlv = ogs_tlv_add(tlv, tlv_mode,
                    desc->type, 0, desc->instance, NULL);
            if (!tlv) {
                ogs_error("ogs_tlv_add()");
                return NULL;
            }
        }
        break;
    }
    default:
        ogs_error("Unknown type [%d]", desc->ctype);
        return NULL;
    }

    return tlv;
}

static uint32_t tlv_add_compound(ogs_tlv_t **root, ogs_tlv_t *parent_tlv,
        ogs_tlv_desc_t *parent_desc, void *msg, int depth, uint8_t mode)
{
    ogs_tlv_presence_t *presence_p;
    ogs_tlv_desc_t *desc = NULL, *next_desc = NULL;
    ogs_tlv_t *tlv = NULL, *emb_tlv = NULL;
    uint8_t *p = msg;
    uint32_t offset = 0, count = 0;
    int i, j, r;
    char indent[17] = "                "; /* 16 spaces */

    ogs_assert(root);
    ogs_assert(parent_desc);
    ogs_assert(msg);

    ogs_assert(depth <= 8);
    indent[depth*2] = 0;

    *root = NULL;

    for (i = 0, desc = parent_desc->child_descs[i]; desc != NULL;
            i++, desc = parent_desc->child_descs[i]) {
        next_desc = parent_desc->child_descs[i+1];
        if (next_desc != NULL && next_desc->ctype == OGS_TLV_MORE) {
            int offset2 = offset;
            for (j = 0; j < next_desc->length; j++) {
                presence_p = (ogs_tlv_presence_t *)(p + offset2);

                if (*presence_p == 0)
                    break;

                if (desc->ctype == OGS_TLV_COMPOUND) {
                    ogs_trace("BUILD %sC#%d [%s] T:%d I:%d (vsz=%d) off:%p ",
                            indent, i, desc->name, desc->type, desc->instance,
                            desc->vsize, p + offset2);

                    if (parent_tlv)
                        tlv = ogs_tlv_embed(parent_tlv,
                                tlv_ctype2mode(desc->ctype, mode),
                                desc->type, 0, desc->instance, NULL);
                    else
                        tlv = ogs_tlv_add(tlv,
                                tlv_ctype2mode(desc->ctype, mode),
                                desc->type, 0, desc->instance, NULL);

                    r = tlv_add_compound(&emb_tlv, tlv, desc,
                            p + offset2 + sizeof(ogs_tlv_presence_t),
                            depth + 1, mode);
                    if (r <= 0 || !emb_tlv) {
                        ogs_error("tlv_add_compound() failed");
                        return 0;
                    }
                    count += 1 + r;
                } else {
                    ogs_trace("BUILD %sL#%d [%s] T:%d L:%d I:%d "
                            "(cls:%d vsz:%d) off:%p ",
                            indent, i, desc->name, desc->type, desc->length,
                            desc->instance, desc->ctype, desc->vsize,
                            p + offset2);

                    tlv = tlv_add_leaf(parent_tlv, tlv, desc,
                            p + offset2, mode);
                    if (!tlv) {
                        ogs_error("tlv_add_leaf() failed");
                        return 0;
                    }
                    count++;
                }

                if (*root == NULL)
                    *root = tlv;

                offset2 += desc->vsize;
            }
            offset += desc->vsize * next_desc->length;
            i++;
        } else {
            presence_p = (ogs_tlv_presence_t *)(p + offset);

            if (*presence_p) {
                if (desc->ctype == OGS_TLV_COMPOUND) {
                    ogs_trace("BUILD %sC#%d [%s] T:%d I:%d (vsz=%d) off:%p ",
                            indent, i, desc->name, desc->type, desc->instance,
                            desc->vsize, p + offset);

                    if (parent_tlv)
                        tlv = ogs_tlv_embed(parent_tlv,
                                tlv_ctype2mode(desc->ctype, mode),
                                desc->type, 0, desc->instance, NULL);
                    else
                        tlv = ogs_tlv_add(tlv,
                                tlv_ctype2mode(desc->ctype, mode),
                                desc->type, 0, desc->instance, NULL);

                    r = tlv_add_compound(&emb_tlv, tlv, desc,
                            p + offset + sizeof(ogs_tlv_presence_t),
                            depth + 1, mode);
                    if (r <= 0 || !emb_tlv) {
                        ogs_error("tlv_add_compound() failed");
                        return 0;
                    }
                    count += 1 + r;
                } else {
                    ogs_trace("BUILD %sL#%d [%s] T:%d L:%d I:%d "
                            "(cls:%d vsz:%d) off:%p ",
                            indent, i, desc->name, desc->type, desc->length,
                            desc->instance, desc->ctype, desc->vsize,
                            p + offset);

                    tlv = tlv_add_leaf(parent_tlv, tlv, desc, p + offset, mode);
                    if (!tlv) {
                        ogs_error("tlv_add_leaf() failed");
                        return 0;
                    }
                    count++;
                }

                if (*root == NULL)
                    *root = tlv;
            }
            offset += desc->vsize;
        }
    }

    return count;
}

ogs_pkbuf_t *ogs_tlv_build_msg(ogs_tlv_desc_t *desc, void *msg, int mode)
{
    ogs_tlv_t *root = NULL;
    uint32_t r, length, rendlen;
    ogs_pkbuf_t *pkbuf = NULL;

    ogs_assert(desc);
    ogs_assert(msg);

    ogs_assert(desc->ctype == OGS_TLV_MESSAGE);

    if (desc->child_descs[0]) {
        r = tlv_add_compound(&root, NULL, desc, msg, 0, mode);
        if (r <= 0 || !root) {
            ogs_error("tlv_add_compound() failed");
            return NULL;
        }

        length = ogs_tlv_calc_length(root);
    } else {
        length = 0;
    }
    pkbuf = ogs_pkbuf_alloc(NULL, OGS_TLV_MAX_HEADROOM+length);
    if (!pkbuf) {
        ogs_error("ogs_pkbuf_alloc() failed");
        return NULL;
    }
    ogs_pkbuf_reserve(pkbuf, OGS_TLV_MAX_HEADROOM);
    ogs_pkbuf_put(pkbuf, length);

    if (desc->child_descs[0]) {
        rendlen = ogs_tlv_render(root, pkbuf->data, length);
        if (rendlen != length) {
            ogs_error("ogs_tlv_render[rendlen:%d != length:%d] failed",
                    rendlen, length);
            return NULL;
        }

        ogs_tlv_free_all(root);
    }

    return pkbuf;
}

static ogs_tlv_desc_t* tlv_find_desc_by_type_inst(uint8_t *desc_index,
        uint32_t *tlv_offset, ogs_tlv_desc_t *parent_desc, uint16_t match_type, uint8_t match_instance, uint8_t match_type_pos)
{
    ogs_tlv_desc_t *prev_desc = NULL, *desc = NULL;
    int i, offset = 0;
    unsigned match_i = 0;

    ogs_assert(parent_desc);

    for (i = 0, desc = parent_desc->child_descs[i]; desc != NULL;
            i++, desc = parent_desc->child_descs[i]) {
        if (desc->type == match_type && desc->instance == match_instance) {
            if (match_i == match_type_pos) {
                *desc_index = i;
                *tlv_offset = offset;
                break;
            }
            match_i++;
        }

        if (desc->ctype == OGS_TLV_MORE) {
            ogs_assert(prev_desc && prev_desc->ctype != OGS_TLV_MORE);
            offset += prev_desc->vsize * (desc->length - 1);
        } else {
            offset += desc->vsize;
        }

        prev_desc = desc;
    }

    return desc;
}

static int tlv_parse_leaf(void *msg, ogs_tlv_desc_t *desc, ogs_tlv_t *tlv)
{
    ogs_assert(msg);
    ogs_assert(desc);
    ogs_assert(tlv);

    switch (desc->ctype) {
    case OGS_TV_UINT8:
    case OGS_TV_INT8:
    case OGS_TLV_UINT8:
    case OGS_TLV_INT8:
    {
        ogs_tlv_uint8_t *v = (ogs_tlv_uint8_t *)msg;

        if (tlv->length != 1) {
            ogs_error("Invalid TLV length %d. It should be 1", tlv->length);
            return OGS_ERROR;
        }
        v->u8 = *(uint8_t*)(tlv->value);
        break;
    }
    case OGS_TV_UINT16:
    case OGS_TV_INT16:
    case OGS_TLV_UINT16:
    case OGS_TLV_INT16:
    {
        ogs_tlv_uint16_t *v = (ogs_tlv_uint16_t *)msg;

        if (tlv->length < 1 || tlv->length > 2) {
            ogs_error("Invalid TLV length %d.", tlv->length);
            return OGS_ERROR;
        }
        v->u16 = ((((uint8_t*)tlv->value)[0]<< 8)&0xff00) |
               ((((uint8_t*)tlv->value)[1]    )&0x00ff);
        break;
    }
    case OGS_TV_UINT24:
    case OGS_TV_INT24:
    case OGS_TLV_UINT24:
    case OGS_TLV_INT24:
    {
        ogs_tlv_uint24_t *v = (ogs_tlv_uint24_t *)msg;

        if (tlv->length < 1 || tlv->length > 3) {
            ogs_error("Invalid TLV length %d.", tlv->length);
            return OGS_ERROR;
        }
        v->u24 = ((((uint8_t*)tlv->value)[0]<<16)&0x00ff0000) |
               ((((uint8_t*)tlv->value)[1]<< 8)&0x0000ff00) |
               ((((uint8_t*)tlv->value)[2]    )&0x000000ff);
        break;
    }
    case OGS_TV_UINT32:
    case OGS_TV_INT32:
    case OGS_TLV_UINT32:
    case OGS_TLV_INT32:
    {
        ogs_tlv_uint32_t *v = (ogs_tlv_uint32_t *)msg;

        if (tlv->length < 1 || tlv->length > 4) {
            ogs_error("Invalid TLV length %d.", tlv->length);
            return OGS_ERROR;
        }
        v->u32 = ((((uint8_t*)tlv->value)[0]<<24)&0xff000000) |
               ((((uint8_t*)tlv->value)[1]<<16)&0x00ff0000) |
               ((((uint8_t*)tlv->value)[2]<< 8)&0x0000ff00) |
               ((((uint8_t*)tlv->value)[3]    )&0x000000ff);
        break;
    }
    case OGS_TV_FIXED_STR:
    case OGS_TLV_FIXED_STR:
    {
        ogs_tlv_octet_t *v = (ogs_tlv_octet_t *)msg;

        if (tlv->length != desc->length)
        {
            ogs_error("Invalid TLV length %d. It should be %d",
                    tlv->length, desc->length);
            return OGS_ERROR;
        }

        v->data = tlv->value;
        v->len = tlv->length;
        break;
    }
    case OGS_TLV_VAR_STR:
    {
        ogs_tlv_octet_t *v = (ogs_tlv_octet_t *)msg;

        v->data = tlv->value;
        v->len = tlv->length;
        break;
    }
    case OGS_TV_NULL:
    case OGS_TLV_NULL:
    {
        if (tlv->length != 0) {
            ogs_error("Invalid TLV length %d. It should be 0", tlv->length);
            return OGS_ERROR;
        }
        break;
    }
    default:
        ogs_error("Unknown type[%d]", desc->ctype);
        return OGS_ERROR;
    }

    return OGS_OK;
}

/* rbtree structure used to keep track count of TLVs with given <type,instance>
 * while parsing. This is used to link it to the matching nth field in a
 * ogs_tlv_desc_t struct */
typedef struct tlv_count_node_s {
    ogs_rbnode_t node;
    uint32_t key; /* type<<8 + instance */
    unsigned count;
} tlv_count_node_t;

static tlv_count_node_t *tlv_count_node_find(ogs_rbtree_t *tree, tlv_count_node_t *count_node_arr, unsigned count_node_arr_len,
        unsigned *count_alloc_next, uint16_t type, uint8_t instance)
{
    ogs_rbnode_t **new = NULL;
    ogs_rbnode_t *parent = NULL;
    tlv_count_node_t *this;

    uint64_t key = (((uint32_t)type)<<8) | instance;

    new = &tree->root;
    while (*new) {
        this = ogs_rb_entry(*new, tlv_count_node_t, node);
        parent = *new;
        if (key < this->key) {
            new = &(*new)->left;
        } else if (key > this->key) {
            new = &(*new)->right;
        } else { /* found entry: */
            return this;
        }
    }

    /* No entry, need to add one: */
    if (*count_alloc_next == count_node_arr_len) {
        ogs_error("This TLV has to many entries, can't parse");
        return NULL;
    }
    this = &count_node_arr[(*count_alloc_next)++];
    this->key = key;
    this->count = 0;
    ogs_rbtree_link_node(&this->node, parent, new);
    ogs_rbtree_insert_color(tree, &this->node);
    return this;
}

static int tlv_parse_compound(void *msg, ogs_tlv_desc_t *parent_desc,
        ogs_tlv_t *parent_tlv, int depth, int mode)
{
    int rv;
    ogs_tlv_presence_t *presence_p = (ogs_tlv_presence_t *)msg;
    ogs_tlv_desc_t *desc = NULL, *next_desc = NULL;
    ogs_tlv_t *tlv = NULL, *emb_tlv = NULL;
    uint8_t *p = msg;
    uint32_t offset = 0;
    uint8_t index = 0;
    int i = 0, j;
    OGS_RBTREE(tree);
    tlv_count_node_t count_node[OGS_TLV_MAX_CHILD_DESC];
    unsigned count_node_alloc_next = 0;
    char indent[17] = "                "; /* 16 spaces */

    ogs_assert(msg);
    ogs_assert(parent_desc);
    ogs_assert(parent_tlv);

    ogs_assert(depth <= 8);
    indent[depth*2] = 0;

    tlv = parent_tlv;
    while (tlv) {
        tlv_count_node_t *curr_count = tlv_count_node_find(&tree, &count_node[0],
                OGS_ARRAY_SIZE(count_node), &count_node_alloc_next, tlv->type, tlv->instance);
        if (!curr_count)
            return OGS_ERROR;
        desc = tlv_find_desc_by_type_inst(&index, &offset, parent_desc, tlv->type, tlv->instance, curr_count->count);
        if (desc == NULL) {
            ogs_warn("Unknown TLV type [%d]", tlv->type);
            tlv = tlv->next;
            continue;
        }

        presence_p = (ogs_tlv_presence_t *)(p + offset);

        /* Multiple of the same type TLV may be included */
        next_desc = parent_desc->child_descs[index+1];
        if (next_desc != NULL && next_desc->ctype == OGS_TLV_MORE) {
            for (j = 0; j < next_desc->length; j++) {
                presence_p =
                    (ogs_tlv_presence_t *)(p + offset + desc->vsize * j);
                if (*presence_p == 0) {
                    offset += desc->vsize * j;
                    break;
                }
            }
            if (j == next_desc->length) {
                ogs_fatal("Multiple of the same type TLV need more room");
                tlv = tlv->next;
                continue;
            }
        } else {
            curr_count->count++;
        }

        if (desc->ctype == OGS_TLV_COMPOUND) {
            emb_tlv = ogs_tlv_parse_embedded_block(tlv, mode);
            if (emb_tlv == NULL) {
                ogs_error("Error while parse TLV");
                return OGS_ERROR;
            }

            ogs_trace("PARSE %sC#%d [%s] T:%d I:%d (vsz=%d) off:%p ",
                    indent, i++, desc->name, desc->type, desc->instance,
                    desc->vsize, p + offset);

            offset += sizeof(ogs_tlv_presence_t);

            rv = tlv_parse_compound(
                    p + offset, desc, emb_tlv, depth + 1, mode);
            if (rv != OGS_OK) {
                ogs_error("Can't parse compound TLV");
                return OGS_ERROR;
            }

            *presence_p = 1;
        } else {
            ogs_trace("PARSE %sL#%d [%s] T:%d L:%d I:%d "
                    "(cls:%d vsz:%d) off:%p ",
                    indent, i++, desc->name, desc->type, desc->length,
                    desc->instance, desc->ctype, desc->vsize, p + offset);

            rv = tlv_parse_leaf(p + offset, desc, tlv);
            if (rv != OGS_OK) {
                ogs_error("Can't parse leaf TLV");
                return OGS_ERROR;
            }

            *presence_p = 1;
        }

        tlv = tlv->next;
    }

    return OGS_OK;
}

int ogs_tlv_parse_msg(void *msg, ogs_tlv_desc_t *desc, ogs_pkbuf_t *pkbuf,
        int mode)
{
    int rv;
    ogs_tlv_t *root;

    ogs_assert(msg);
    ogs_assert(desc);
    ogs_assert(pkbuf);

    ogs_assert(desc->ctype == OGS_TLV_MESSAGE);
    if (!desc->child_descs[0]) {
        ogs_fatal("No Child Descs in [%s]", desc->name);
        ogs_assert_if_reached();
    }

    root = ogs_tlv_parse_block(pkbuf->len, pkbuf->data, mode);
    if (root == NULL) {
        ogs_error("Can't parse TLV message");
        return OGS_ERROR;
    }

    rv = tlv_parse_compound(msg, desc, root, 0, mode);

    ogs_tlv_free_all(root);

    return rv;
}

static uint16_t parse_get_element_type(uint8_t *pos, uint8_t mode)
{
    uint16_t type;

    switch(mode) {
    case OGS_TLV_MODE_T1_L1:
    case OGS_TLV_MODE_T1_L2:
    case OGS_TLV_MODE_T1_L2_I1:
    case OGS_TLV_MODE_T1:
        type = *pos;
        break;
    case OGS_TLV_MODE_T2_L2:
        type = *(pos++) << 8;
        type += *(pos++);
        break;
    default:
        ogs_assert_if_reached();
        break;
    }

    return type;
}

/* Get TLV element taking into account msg_mode (to know TLV tag length) +
 * specific TLV information from "desc" (to know whether the specific IE is TLV
 * or TV, and its expected length in the later case). */
static uint8_t *tlv_get_element_desc(ogs_tlv_t *tlv, uint8_t *blk, uint8_t msg_mode, ogs_tlv_desc_t *desc)
{
    uint8_t instance;
    unsigned tlv_tag_pos;
    uint8_t desc_index = 0;
    uint32_t tlv_offset = 0;
    uint16_t tlv_tag;
    uint8_t tlv_mode;
    static ogs_tlv_desc_t* tlv_desc;

    tlv_tag = parse_get_element_type(blk, msg_mode);
    instance = 0;  /* TODO: support instance != 0 if ever really needed by looking it up in pos */
    tlv_tag_pos = 0; /* All tags with same instance should use the same tlv_desc, so take the first one */
    tlv_desc = tlv_find_desc_by_type_inst(&desc_index, &tlv_offset, desc, tlv_tag, instance, tlv_tag_pos);
    if (!tlv_desc) {
        ogs_error("Can't parse find TLV description for type %u", tlv_tag);
        return NULL;
    }
    tlv_mode = tlv_ctype2mode(tlv_desc->ctype, msg_mode);

    if (tlv_mode == OGS_TLV_MODE_T1)
        return tlv_get_element_fixed(tlv, blk, tlv_mode, tlv_desc->length);
    return tlv_get_element(tlv, blk, tlv_mode);
}

/* Similar to ogs_tlv_parse_block(), but taking into account each TLV format from "desc". */
static ogs_tlv_t *ogs_tlv_parse_block_desc(uint32_t length, void *data, uint8_t msg_mode, ogs_tlv_desc_t *desc)
{
    uint8_t *pos = data;
    uint8_t *blk = data;

    ogs_tlv_t *root = NULL;
    ogs_tlv_t *prev = NULL;
    ogs_tlv_t *curr = NULL;

    root = curr = ogs_tlv_get();

    ogs_assert(curr);

    pos = tlv_get_element_desc(curr, pos, msg_mode, desc);

    ogs_assert(pos);

    while(pos - blk < length) {
        prev = curr;

        curr = ogs_tlv_get();
        ogs_assert(curr);
        prev->next = curr;

        pos = tlv_get_element_desc(curr, pos, msg_mode, desc);
        ogs_assert(pos);
    }

    if (length != (pos - blk)) {
        ogs_error("ogs_tlv_parse_block() failed[LEN:%d,MODE:%d]",
                length, msg_mode);
        ogs_error("POS[%p] BLK[%p] POS-BLK[%d]", pos, blk, (int)(pos - blk));
        ogs_log_hexdump(OGS_LOG_FATAL, data, length);

        ogs_tlv_free_all(root);
        return NULL;
    }

    return root;
}

/* Similar to ogs_tlv_parse_msg(), but takes each TLV type from the desc
 * defintion. This allows parsing messages which have different types of TLVs in
 * it (for instance GTPv1-C). */
int ogs_tlv_parse_msg_desc(
        void *msg, ogs_tlv_desc_t *desc, ogs_pkbuf_t *pkbuf, int msg_mode)
{
    int rv;
    ogs_tlv_t *root;

    ogs_assert(msg);
    ogs_assert(desc);
    ogs_assert(pkbuf);

    ogs_assert(desc->ctype == OGS_TLV_MESSAGE);
    ogs_assert(desc->child_descs[0]);

    root = ogs_tlv_parse_block_desc(pkbuf->len, pkbuf->data, msg_mode, desc);
    if (root == NULL) {
        ogs_error("Can't parse TLV message");
        return OGS_ERROR;
    }

    rv = tlv_parse_compound(msg, desc, root, 0, msg_mode);

    ogs_tlv_free_all(root);

    return rv;
}