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

Source (jump to first uncovered line)
/*
 * 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"

#undef OGS_LOG_DOMAIN
#define OGS_LOG_DOMAIN __ogs_tlv_domain

static OGS_POOL(pool, ogs_tlv_t);

/* ogs_tlv_t common functions */
ogs_tlv_t *ogs_tlv_get(void)
{
    ogs_tlv_t *tlv = NULL;

    /* get tlv node from node pool */
    ogs_pool_alloc(&pool, &tlv);

    /* check for error */
    ogs_assert(tlv);

    /* intialize tlv node */
    memset(tlv, 0, sizeof(ogs_tlv_t));
    return tlv;
}

void ogs_tlv_free(ogs_tlv_t *tlv)
{
    /* free tlv node to the node pool */
    ogs_pool_free(&pool, tlv);
}

void ogs_tlv_init(void)
{
    ogs_pool_init(&pool, ogs_core()->tlv.pool);
}

void ogs_tlv_final(void)
{
    ogs_pool_final(&pool);
}

uint32_t ogs_tlv_pool_avail(void)
{
    return ogs_pool_avail(&pool);
}

void ogs_tlv_free_all(ogs_tlv_t *root)
{
    /* free all tlv node to the node pool */
    ogs_tlv_t *iter = root;
    ogs_tlv_t *next = NULL;
    while (iter) {
        if(iter->embedded != NULL) {
            ogs_tlv_free_all(iter->embedded);
        }
        next = iter->next;
        ogs_tlv_free(iter);
        iter = next;
    }
}

uint8_t ogs_tlv_value_8(ogs_tlv_t *tlv)
{
    return (*((uint8_t*)(tlv->value)));
}

uint16_t ogs_tlv_value_16(ogs_tlv_t *tlv)
{
    uint16_t u_16;
    uint8_t *v = tlv->value;

    u_16 = ((v[0] <<  8) & 0xff00) |
           ((v[1]      ) & 0x00ff);

    return u_16;
}

uint32_t ogs_tlv_value_32(ogs_tlv_t *tlv)
{
    uint32_t u_32;
    uint8_t *v = tlv->value;

    u_32 = ((v[0] << 24) & 0xff000000) |
           ((v[1] << 16) & 0x00ff0000) |
           ((v[2] <<  8) & 0x0000ff00) |
           ((v[3]      ) & 0x000000ff);

    return u_32;
}

uint32_t ogs_tlv_calc_length(ogs_tlv_t *tlv)
{
    ogs_tlv_t *iter = tlv;
    uint32_t length = 0;

    while(iter) {
        /* this is length for type field */
        switch(iter->mode) {
        case OGS_TLV_MODE_T1_L1:
            length += 2;
            break;
        case OGS_TLV_MODE_T1_L2:
            length += 3;
            break;
        case OGS_TLV_MODE_T1_L2_I1:
        case OGS_TLV_MODE_T2_L2:
            length += 4;
            break;
        case OGS_TLV_MODE_T1:
            length += 1;
            break;
        default:
            ogs_assert_if_reached();
            break;
        }

        /* this is length for type field */
        if(iter->embedded != NULL) {
            iter->length = ogs_tlv_calc_length(iter->embedded);
        }

        /* this is length for value field */
        length += iter->length;

        iter = iter->next;
    }
    return length;
}

uint32_t ogs_tlv_calc_count(ogs_tlv_t *tlv)
{
    ogs_tlv_t *iter = tlv;
    uint32_t count = 0;

    while(iter) {
        if(iter->embedded != NULL) {
            count += ogs_tlv_calc_count(iter->embedded);
        } else {
            count++;
        }
        iter = iter->next;
    }
    return count;
}

static uint8_t *tlv_put_type(uint32_t type, uint8_t *pos, uint8_t mode)
{
    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:
        *(pos++) = type & 0xFF;
        break;
    case OGS_TLV_MODE_T2_L2:
        *(pos++) = (type >> 8) & 0xFF;
        *(pos++) = type & 0xFF;
        break;
    default:
        ogs_assert_if_reached();
        break;
    }
    return pos;
}

static uint8_t *tlv_put_length(uint32_t length, uint8_t *pos, uint8_t mode)
{
    switch(mode) {
    case OGS_TLV_MODE_T1_L1:
        *(pos++) = length & 0xFF;
        break;
    case OGS_TLV_MODE_T1_L2:
    case OGS_TLV_MODE_T1_L2_I1:
    case OGS_TLV_MODE_T2_L2:
        *(pos++) = (length >> 8) & 0xFF;
        *(pos++) = length & 0xFF;
        break;
    case OGS_TLV_MODE_T1:
        break;
    default:
        ogs_assert_if_reached();
        break;
    }

    return pos;
}

static uint8_t *tlv_put_instance(uint8_t instance, uint8_t *pos, uint8_t mode)
{
    switch(mode) {
        case OGS_TLV_MODE_T1_L2_I1:
            *(pos++) = instance & 0xFF;
            break;
        default:
            break;
    }

    return pos;
}

uint8_t *tlv_get_element(ogs_tlv_t *tlv, uint8_t *blk, uint8_t mode)
{
    uint8_t *pos = blk;

    tlv->mode = mode;

    switch(mode) {
    case OGS_TLV_MODE_T1_L1:
        tlv->type = *(pos++);
        tlv->length = *(pos++);
        break;
    case OGS_TLV_MODE_T1_L2:
        tlv->type = *(pos++);
        tlv->length = *(pos++) << 8;
        tlv->length += *(pos++);
        break;
    case OGS_TLV_MODE_T1_L2_I1:
        tlv->type = *(pos++);
        tlv->length = *(pos++) << 8;
        tlv->length += *(pos++);
        tlv->instance = *(pos++) & 0b00001111;
        break;
    case OGS_TLV_MODE_T2_L2:
        tlv->type = *(pos++) << 8;
        tlv->type += *(pos++);
        tlv->length = *(pos++) << 8;
        tlv->length += *(pos++);
        break;
    case OGS_TLV_MODE_T1:
        tlv->type = *(pos++);
        tlv->length = 0;
        break;
    default:
        ogs_assert_if_reached();
        break;
    }

    tlv->value = pos;

    return (pos + ogs_tlv_length(tlv));
}

uint8_t *tlv_get_element_fixed(ogs_tlv_t *tlv, uint8_t *blk, uint8_t mode, uint32_t fixed_length)
{
    uint8_t *pos = blk;

    switch(mode) {
    case OGS_TLV_MODE_T1:
        tlv->type = *(pos++);
        tlv->length = fixed_length;
        break;
    default:
        ogs_assert_if_reached();
        break;
    }

    tlv->value = pos;

    return (pos + ogs_tlv_length(tlv));
}

static void tlv_alloc_buff_to_tlv(
        ogs_tlv_t *head, uint8_t *buff, uint32_t buff_len)
{
    head->buff_allocated = true;
    head->buff_len = buff_len;
    head->buff_ptr = buff;
    head->buff = buff;
}

ogs_tlv_t *ogs_tlv_find_root(ogs_tlv_t *tlv)
{
    ogs_tlv_t *head = tlv->head;
    ogs_tlv_t *parent;

    parent = head->parent;
    while(parent) {
        head = parent->head;
        parent = head->parent;
    }

    return head;
}

ogs_tlv_t *ogs_tlv_add(ogs_tlv_t *head, uint8_t mode,
    uint32_t type, uint32_t length, uint8_t instance, void *value)
{
    ogs_tlv_t *curr = head;
    ogs_tlv_t *new = NULL;

    new = ogs_tlv_get();
    ogs_assert(new);
    if(length != 0)
        ogs_assert(value);

    new->mode = mode;
    new->type = type;
    new->length = length;
    new->instance = instance;
    new->value = value;

    if (head != NULL && head->buff_allocated == true) {
        ogs_assert((head->buff_ptr - head->buff + length) < head->buff_len);

        memcpy(head->buff_ptr, value, length);
        new->value = head->buff_ptr;
        head->buff_ptr += length;
    }

    if(curr == NULL) {
        new->head = new;
        new->tail = new;
    } else {
        head = head->head; /* in case head is not head */
        new->head = head;
        head->tail->next = new;
        head->tail = new;
    }
    return new;
}

ogs_tlv_t *ogs_tlv_copy(void *buff, uint32_t buff_len, uint8_t mode,
    uint32_t type, uint32_t length, uint8_t instance, void *value)
{
    ogs_tlv_t *new = NULL;

    new = ogs_tlv_get();
    ogs_assert(new);

    new->mode = mode;
    new->type = type;
    new->length = length;
    new->instance = instance;
    new->value = value;
    new->head = new->tail = new;

    tlv_alloc_buff_to_tlv(new, buff, buff_len);

    memcpy(new->buff_ptr, value, length);
    new->value = new->buff_ptr;
    new->buff_ptr += length;

    return new;
}

ogs_tlv_t *ogs_tlv_embed(ogs_tlv_t *parent, uint8_t mode,
    uint32_t type, uint32_t length, uint8_t instance, void *value)
{
    ogs_tlv_t *new = NULL, *root = NULL;

    ogs_assert(parent);

    new = ogs_tlv_get();
    ogs_assert(new);

    new->mode = mode;
    new->type = type;
    new->length = length;
    new->instance = instance;
    new->value = value;

    root = ogs_tlv_find_root(parent);

    if(root->buff_allocated == true) {
        ogs_assert((root->buff_ptr - root->buff + length) < root->buff_len);

        memcpy(root->buff_ptr, value, length);
        new->value = root->buff_ptr;
        root->buff_ptr += length;
    }

    if(parent->embedded == NULL) {
        parent->embedded = new->head = new->tail = new;
        new->parent = parent;
    } else {
        new->head = parent->embedded;
        parent->embedded->tail->next = new;
        parent->embedded->tail = new;
    }

    return new;
}

uint32_t ogs_tlv_render(ogs_tlv_t *root, void *data, uint32_t length)
{
    ogs_tlv_t *curr = root;
    uint8_t *pos = data;
    uint8_t *blk = data;
    uint32_t embedded_len = 0;

    while(curr) {
        pos = tlv_put_type(curr->type, pos, curr->mode);

        if(curr->embedded == NULL) {
            pos = tlv_put_length(curr->length, pos, curr->mode);
            pos = tlv_put_instance(curr->instance, pos, curr->mode);

            if ((pos - blk) + ogs_tlv_length(curr) > length)
                ogs_assert_if_reached();

            memcpy((char*)pos, (char*)curr->value, curr->length);
            pos += curr->length;
        } else {
            embedded_len = ogs_tlv_calc_length(curr->embedded);
            pos = tlv_put_length(embedded_len, pos, curr->mode);
            pos = tlv_put_instance(curr->instance, pos, curr->mode);
            ogs_tlv_render(curr->embedded,
                pos, length - (uint32_t)(pos-blk));
            pos += embedded_len;
        }
        curr = curr->next;
    }

    return (pos - blk);
}

/* ogs_tlv_t parsing functions */
ogs_tlv_t *ogs_tlv_parse_block(uint32_t length, void *data, uint8_t mode)
{
    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(curr, pos, mode);

    ogs_assert(pos);

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

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

        pos = tlv_get_element(curr, pos, mode);
        ogs_assert(pos);
    }

    if (length != (pos - blk)) {
        ogs_error("ogs_tlv_parse_block() failed[LEN:%d,MODE:%d]", length, 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;
}

ogs_tlv_t *ogs_tlv_parse_embedded_block(ogs_tlv_t *tlv, uint8_t mode)
{
    tlv->embedded = ogs_tlv_parse_block(tlv->length, tlv->value, mode);

    return tlv->embedded;
}

/* tlv operation-related function */
ogs_tlv_t *ogs_tlv_find(ogs_tlv_t *root, uint32_t type)
{
    ogs_tlv_t *iter = root, *embed = NULL;
    while(iter) {
        if(iter->type == type) {
            return iter;
        }

        if(iter->embedded != NULL) {
            embed = ogs_tlv_find(iter->embedded, type);
            if(embed != NULL) {
                return embed;
            }
        }
        iter = iter->next;
    }

    /* tlv for the designated type doesn't exist */
    return NULL;
}

Coverage Report

Created: 2023-09-25 07:12

Line	Count	Source (jump to first uncovered line)
1		/*
2		* Copyright (C) 2019 by Sukchan Lee <acetcom@gmail.com>
3		* Copyright (C) 2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
4		*
5		* This file is part of Open5GS.
6		*
7		* This program is free software: you can redistribute it and/or modify
8		* it under the terms of the GNU Affero General Public License as published by
9		* the Free Software Foundation, either version 3 of the License, or
10		* (at your option) any later version.
11		*
12		* This program is distributed in the hope that it will be useful,
13		* but WITHOUT ANY WARRANTY; without even the implied warranty of
14		* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15		* GNU General Public License for more details.
16		*
17		* You should have received a copy of the GNU General Public License
18		* along with this program. If not, see <https://www.gnu.org/licenses/>.
19		*/
20
21		#include "ogs-core.h"
22
23		#undef OGS_LOG_DOMAIN
24	486	#define OGS_LOG_DOMAIN __ogs_tlv_domain
25
26		static OGS_POOL(pool, ogs_tlv_t);
27
28		/* ogs_tlv_t common functions */
29		ogs_tlv_t *ogs_tlv_get(void)
30	7.92k	{
31	7.92k	ogs_tlv_t *tlv = NULL;
32
33		/* get tlv node from node pool */
34	7.92k	ogs_pool_alloc(&pool, &tlv);
35
36		/* check for error */
37	7.92k	ogs_assert(tlv);
38
39		/* intialize tlv node */
40	7.92k	memset(tlv, 0, sizeof(ogs_tlv_t));
41	7.92k	return tlv;
42	7.92k	}
43
44		void ogs_tlv_free(ogs_tlv_t *tlv)
45	7.92k	{
46		/* free tlv node to the node pool */
47	7.92k	ogs_pool_free(&pool, tlv);
48	7.92k	}
49
50		void ogs_tlv_init(void)
51	2	{
52	2	ogs_pool_init(&pool, ogs_core()->tlv.pool);
53	2	}
54
55		void ogs_tlv_final(void)
56	0	{
57	0	ogs_pool_final(&pool);
58	0	}
59
60		uint32_t ogs_tlv_pool_avail(void)
61	0	{
62	0	return ogs_pool_avail(&pool);
63	0	}
64
65		void ogs_tlv_free_all(ogs_tlv_t *root)
66	818	{
67		/* free all tlv node to the node pool */
68	818	ogs_tlv_t *iter = root;
69	818	ogs_tlv_t *next = NULL;
70	8.74k	while (iter) {
71	7.92k	if(iter->embedded != NULL) {
72	335	ogs_tlv_free_all(iter->embedded);
73	335	}
74	7.92k	next = iter->next;
75	7.92k	ogs_tlv_free(iter);
76	7.92k	iter = next;
77	7.92k	}
78	818	}
79
80		uint8_t ogs_tlv_value_8(ogs_tlv_t *tlv)
81	0	{
82	0	return (((uint8_t)(tlv->value)));
83	0	}
84
85		uint16_t ogs_tlv_value_16(ogs_tlv_t *tlv)
86	0	{
87	0	uint16_t u_16;
88	0	uint8_t *v = tlv->value;
89
90	0	u_16 = ((v[0] << 8) & 0xff00) \|
91	0	((v[1] ) & 0x00ff);
92
93	0	return u_16;
94	0	}
95
96		uint32_t ogs_tlv_value_32(ogs_tlv_t *tlv)
97	0	{
98	0	uint32_t u_32;
99	0	uint8_t *v = tlv->value;
100
101	0	u_32 = ((v[0] << 24) & 0xff000000) \|
102	0	((v[1] << 16) & 0x00ff0000) \|
103	0	((v[2] << 8) & 0x0000ff00) \|
104	0	((v[3] ) & 0x000000ff);
105
106	0	return u_32;
107	0	}
108
109		uint32_t ogs_tlv_calc_length(ogs_tlv_t *tlv)
110	0	{
111	0	ogs_tlv_t *iter = tlv;
112	0	uint32_t length = 0;
113
114	0	while(iter) {
115		/* this is length for type field */
116	0	switch(iter->mode) {
117	0	case OGS_TLV_MODE_T1_L1:
118	0	length += 2;
119	0	break;
120	0	case OGS_TLV_MODE_T1_L2:
121	0	length += 3;
122	0	break;
123	0	case OGS_TLV_MODE_T1_L2_I1:
124	0	case OGS_TLV_MODE_T2_L2:
125	0	length += 4;
126	0	break;
127	0	case OGS_TLV_MODE_T1:
128	0	length += 1;
129	0	break;
130	0	default:
131	0	ogs_assert_if_reached();
132	0	break;
133	0	}
134
135		/* this is length for type field */
136	0	if(iter->embedded != NULL) {
137	0	iter->length = ogs_tlv_calc_length(iter->embedded);
138	0	}
139
140		/* this is length for value field */
141	0	length += iter->length;
142
143	0	iter = iter->next;
144	0	}
145	0	return length;
146	0	}
147
148		uint32_t ogs_tlv_calc_count(ogs_tlv_t *tlv)
149	0	{
150	0	ogs_tlv_t *iter = tlv;
151	0	uint32_t count = 0;
152
153	0	while(iter) {
154	0	if(iter->embedded != NULL) {
155	0	count += ogs_tlv_calc_count(iter->embedded);
156	0	} else {
157	0	count++;
158	0	}
159	0	iter = iter->next;
160	0	}
161	0	return count;
162	0	}
163
164		static uint8_t tlv_put_type(uint32_t type, uint8_t pos, uint8_t mode)
165	0	{
166	0	switch(mode) {
167	0	case OGS_TLV_MODE_T1_L1:
168	0	case OGS_TLV_MODE_T1_L2:
169	0	case OGS_TLV_MODE_T1_L2_I1:
170	0	case OGS_TLV_MODE_T1:
171	0	*(pos++) = type & 0xFF;
172	0	break;
173	0	case OGS_TLV_MODE_T2_L2:
174	0	*(pos++) = (type >> 8) & 0xFF;
175	0	*(pos++) = type & 0xFF;
176	0	break;
177	0	default:
178	0	ogs_assert_if_reached();
179	0	break;
180	0	}
181	0	return pos;
182	0	}
183
184		static uint8_t tlv_put_length(uint32_t length, uint8_t pos, uint8_t mode)
185	0	{
186	0	switch(mode) {
187	0	case OGS_TLV_MODE_T1_L1:
188	0	*(pos++) = length & 0xFF;
189	0	break;
190	0	case OGS_TLV_MODE_T1_L2:
191	0	case OGS_TLV_MODE_T1_L2_I1:
192	0	case OGS_TLV_MODE_T2_L2:
193	0	*(pos++) = (length >> 8) & 0xFF;
194	0	*(pos++) = length & 0xFF;
195	0	break;
196	0	case OGS_TLV_MODE_T1:
197	0	break;
198	0	default:
199	0	ogs_assert_if_reached();
200	0	break;
201	0	}
202
203	0	return pos;
204	0	}
205
206		static uint8_t tlv_put_instance(uint8_t instance, uint8_t pos, uint8_t mode)
207	0	{
208	0	switch(mode) {
209	0	case OGS_TLV_MODE_T1_L2_I1:
210	0	*(pos++) = instance & 0xFF;
211	0	break;
212	0	default:
213	0	break;
214	0	}
215
216	0	return pos;
217	0	}
218
219		uint8_t tlv_get_element(ogs_tlv_t tlv, uint8_t *blk, uint8_t mode)
220	7.92k	{
221	7.92k	uint8_t *pos = blk;
222
223	7.92k	tlv->mode = mode;
224
225	7.92k	switch(mode) {
226	0	case OGS_TLV_MODE_T1_L1:
227	0	tlv->type = *(pos++);
228	0	tlv->length = *(pos++);
229	0	break;
230	0	case OGS_TLV_MODE_T1_L2:
231	0	tlv->type = *(pos++);
232	0	tlv->length = *(pos++) << 8;
233	0	tlv->length += *(pos++);
234	0	break;
235	7.92k	case OGS_TLV_MODE_T1_L2_I1:
236	7.92k	tlv->type = *(pos++);
237	7.92k	tlv->length = *(pos++) << 8;
238	7.92k	tlv->length += *(pos++);
239	7.92k	tlv->instance = *(pos++) & 0b00001111;
240	7.92k	break;
241	0	case OGS_TLV_MODE_T2_L2:
242	0	tlv->type = *(pos++) << 8;
243	0	tlv->type += *(pos++);
244	0	tlv->length = *(pos++) << 8;
245	0	tlv->length += *(pos++);
246	0	break;
247	0	case OGS_TLV_MODE_T1:
248	0	tlv->type = *(pos++);
249	0	tlv->length = 0;
250	0	break;
251	0	default:
252	0	ogs_assert_if_reached();
253	0	break;
254	7.92k	}
255
256	7.92k	tlv->value = pos;
257
258	7.92k	return (pos + ogs_tlv_length(tlv));
259	7.92k	}
260
261		uint8_t tlv_get_element_fixed(ogs_tlv_t tlv, uint8_t *blk, uint8_t mode, uint32_t fixed_length)
262	0	{
263	0	uint8_t *pos = blk;
264
265	0	switch(mode) {
266	0	case OGS_TLV_MODE_T1:
267	0	tlv->type = *(pos++);
268	0	tlv->length = fixed_length;
269	0	break;
270	0	default:
271	0	ogs_assert_if_reached();
272	0	break;
273	0	}
274
275	0	tlv->value = pos;
276
277	0	return (pos + ogs_tlv_length(tlv));
278	0	}
279
280		static void tlv_alloc_buff_to_tlv(
281		ogs_tlv_t head, uint8_t buff, uint32_t buff_len)
282	0	{
283	0	head->buff_allocated = true;
284	0	head->buff_len = buff_len;
285	0	head->buff_ptr = buff;
286	0	head->buff = buff;
287	0	}
288
289		ogs_tlv_t ogs_tlv_find_root(ogs_tlv_t tlv)
290	0	{
291	0	ogs_tlv_t *head = tlv->head;
292	0	ogs_tlv_t *parent;
293
294	0	parent = head->parent;
295	0	while(parent) {
296	0	head = parent->head;
297	0	parent = head->parent;
298	0	}
299
300	0	return head;
301	0	}
302
303		ogs_tlv_t ogs_tlv_add(ogs_tlv_t head, uint8_t mode,
304		uint32_t type, uint32_t length, uint8_t instance, void *value)
305	0	{
306	0	ogs_tlv_t *curr = head;
307	0	ogs_tlv_t *new = NULL;
308
309	0	new = ogs_tlv_get();
310	0	ogs_assert(new);
311	0	if(length != 0)
312	0	ogs_assert(value);
313
314	0	new->mode = mode;
315	0	new->type = type;
316	0	new->length = length;
317	0	new->instance = instance;
318	0	new->value = value;
319
320	0	if (head != NULL && head->buff_allocated == true) {
321	0	ogs_assert((head->buff_ptr - head->buff + length) < head->buff_len);
322
323	0	memcpy(head->buff_ptr, value, length);
324	0	new->value = head->buff_ptr;
325	0	head->buff_ptr += length;
326	0	}
327
328	0	if(curr == NULL) {
329	0	new->head = new;
330	0	new->tail = new;
331	0	} else {
332	0	head = head->head; /* in case head is not head */
333	0	new->head = head;
334	0	head->tail->next = new;
335	0	head->tail = new;
336	0	}
337	0	return new;
338	0	}
339
340		ogs_tlv_t ogs_tlv_copy(void buff, uint32_t buff_len, uint8_t mode,
341		uint32_t type, uint32_t length, uint8_t instance, void *value)
342	0	{
343	0	ogs_tlv_t *new = NULL;
344
345	0	new = ogs_tlv_get();
346	0	ogs_assert(new);
347
348	0	new->mode = mode;
349	0	new->type = type;
350	0	new->length = length;
351	0	new->instance = instance;
352	0	new->value = value;
353	0	new->head = new->tail = new;
354
355	0	tlv_alloc_buff_to_tlv(new, buff, buff_len);
356
357	0	memcpy(new->buff_ptr, value, length);
358	0	new->value = new->buff_ptr;
359	0	new->buff_ptr += length;
360
361	0	return new;
362	0	}
363
364		ogs_tlv_t ogs_tlv_embed(ogs_tlv_t parent, uint8_t mode,
365		uint32_t type, uint32_t length, uint8_t instance, void *value)
366	0	{
367	0	ogs_tlv_t new = NULL, root = NULL;
368
369	0	ogs_assert(parent);
370
371	0	new = ogs_tlv_get();
372	0	ogs_assert(new);
373
374	0	new->mode = mode;
375	0	new->type = type;
376	0	new->length = length;
377	0	new->instance = instance;
378	0	new->value = value;
379
380	0	root = ogs_tlv_find_root(parent);
381
382	0	if(root->buff_allocated == true) {
383	0	ogs_assert((root->buff_ptr - root->buff + length) < root->buff_len);
384
385	0	memcpy(root->buff_ptr, value, length);
386	0	new->value = root->buff_ptr;
387	0	root->buff_ptr += length;
388	0	}
389
390	0	if(parent->embedded == NULL) {
391	0	parent->embedded = new->head = new->tail = new;
392	0	new->parent = parent;
393	0	} else {
394	0	new->head = parent->embedded;
395	0	parent->embedded->tail->next = new;
396	0	parent->embedded->tail = new;
397	0	}
398
399	0	return new;
400	0	}
401
402		uint32_t ogs_tlv_render(ogs_tlv_t root, void data, uint32_t length)
403	0	{
404	0	ogs_tlv_t *curr = root;
405	0	uint8_t *pos = data;
406	0	uint8_t *blk = data;
407	0	uint32_t embedded_len = 0;
408
409	0	while(curr) {
410	0	pos = tlv_put_type(curr->type, pos, curr->mode);
411
412	0	if(curr->embedded == NULL) {
413	0	pos = tlv_put_length(curr->length, pos, curr->mode);
414	0	pos = tlv_put_instance(curr->instance, pos, curr->mode);
415
416	0	if ((pos - blk) + ogs_tlv_length(curr) > length)
417	0	ogs_assert_if_reached();
418
419	0	memcpy((char)pos, (char)curr->value, curr->length);
420	0	pos += curr->length;
421	0	} else {
422	0	embedded_len = ogs_tlv_calc_length(curr->embedded);
423	0	pos = tlv_put_length(embedded_len, pos, curr->mode);
424	0	pos = tlv_put_instance(curr->instance, pos, curr->mode);
425	0	ogs_tlv_render(curr->embedded,
426	0	pos, length - (uint32_t)(pos-blk));
427	0	pos += embedded_len;
428	0	}
429	0	curr = curr->next;
430	0	}
431
432	0	return (pos - blk);
433	0	}
434
435		/* ogs_tlv_t parsing functions */
436		ogs_tlv_t ogs_tlv_parse_block(uint32_t length, void data, uint8_t mode)
437	818	{
438	818	uint8_t *pos = data;
439	818	uint8_t *blk = data;
440
441	818	ogs_tlv_t *root = NULL;
442	818	ogs_tlv_t *prev = NULL;
443	818	ogs_tlv_t *curr = NULL;
444
445	818	root = curr = ogs_tlv_get();
446
447	818	ogs_assert(curr);
448
449	818	pos = tlv_get_element(curr, pos, mode);
450
451	818	ogs_assert(pos);
452
453	7.92k	while(pos - blk < length) {
454	7.10k	prev = curr;
455
456	7.10k	curr = ogs_tlv_get();
457	7.10k	ogs_assert(curr);
458	7.10k	prev->next = curr;
459
460	7.10k	pos = tlv_get_element(curr, pos, mode);
461	7.10k	ogs_assert(pos);
462	7.10k	}
463
464	818	if (length != (pos - blk)) {
465	162	ogs_error("ogs_tlv_parse_block() failed[LEN:%d,MODE:%d]", length, mode);
466	162	ogs_error("POS[%p] BLK[%p] POS-BLK[%d]", pos, blk, (int)(pos - blk));
467	162	ogs_log_hexdump(OGS_LOG_FATAL, data, length);
468
469	162	ogs_tlv_free_all(root);
470	162	return NULL;
471	162	}
472
473	656	return root;
474	818	}
475
476		ogs_tlv_t ogs_tlv_parse_embedded_block(ogs_tlv_t tlv, uint8_t mode)
477	369	{
478	369	tlv->embedded = ogs_tlv_parse_block(tlv->length, tlv->value, mode);
479
480	369	return tlv->embedded;
481	369	}
482
483		/* tlv operation-related function */
484		ogs_tlv_t ogs_tlv_find(ogs_tlv_t root, uint32_t type)
485	0	{
486	0	ogs_tlv_t iter = root, embed = NULL;
487	0	while(iter) {
488	0	if(iter->type == type) {
489	0	return iter;
490	0	}
491
492	0	if(iter->embedded != NULL) {
493	0	embed = ogs_tlv_find(iter->embedded, type);
494	0	if(embed != NULL) {
495	0	return embed;
496	0	}
497	0	}
498	0	iter = iter->next;
499	0	}
500
501		/* tlv for the designated type doesn't exist */
502	0	return NULL;
503	0	}