891 lines
27 KiB
C
891 lines
27 KiB
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;
|
||
|
}
|