[proxmark3-svn] / client / emv / tlv.c

/*
 * libopenemv - a library to work with EMV family of smart cards
 * Copyright (C) 2012, 2015 Dmitry Eremin-Solenikov
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 * 
 * https://github.com/lumag/emv-tools/blob/master/lib/tlv.c
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "tlv.h"

#include <string.h>
#include <stdint.h>
#include <stddef.h>
#include <stdlib.h>

#define TLV_TAG_CLASS_MASK	0xc0
#define TLV_TAG_COMPLEX		0x20
#define TLV_TAG_VALUE_MASK	0x1f
#define TLV_TAG_VALUE_CONT	0x1f
#define TLV_TAG_INVALID		0

#define TLV_LEN_LONG		0x80
#define TLV_LEN_MASK		0x7f
#define TLV_LEN_INVALID		(~0)

// http://radek.io/2012/11/10/magical-container_of-macro/
//#define container_of(ptr, type, member) ({			       
//	const typeof( ((type *)0)->member ) *__mptr = (ptr);	
//        (type *)( (char *)__mptr - offsetof(type,member) );})

struct tlvdb {
	struct tlv tag;
	struct tlvdb *next;
	struct tlvdb *parent;
	struct tlvdb *children;
};

struct tlvdb_root {
	struct tlvdb db;
	size_t len;
	unsigned char buf[0];
};

static tlv_tag_t tlv_parse_tag(const unsigned char **buf, size_t *len)
{
	tlv_tag_t tag;

	if (*len == 0)
		return TLV_TAG_INVALID;
	tag = **buf;
	--*len;
	++*buf;
	if ((tag & TLV_TAG_VALUE_MASK) != TLV_TAG_VALUE_CONT)
		return tag;

	if (*len == 0)
		return TLV_TAG_INVALID;

	tag <<= 8;
	tag |= **buf;
	--*len;
	++*buf;

	return tag;
}

static size_t tlv_parse_len(const unsigned char **buf, size_t *len)
{
	size_t l;

	if (*len == 0)
		return TLV_LEN_INVALID;

	l = **buf;
	--*len;
	++*buf;

	if (!(l & TLV_LEN_LONG))
		return l;

	size_t ll = l &~ TLV_LEN_LONG;
	if (ll > 5)
		return TLV_LEN_INVALID;

	l = 0;
	for (int i = 1; i <= ll; i++) {
		l = (l << 8) + **buf;
		--*len;
		++*buf;
	}

	return l;
}

bool tlv_parse_tl(const unsigned char **buf, size_t *len, struct tlv *tlv)
{
	tlv->value = 0;

	tlv->tag = tlv_parse_tag(buf, len);
	if (tlv->tag == TLV_TAG_INVALID)
		return false;

	tlv->len = tlv_parse_len(buf, len);
	if (tlv->len == TLV_LEN_INVALID)
		return false;

	return true;
}

static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent);

static bool tlvdb_parse_one(struct tlvdb *tlvdb,
		struct tlvdb *parent,
		const unsigned char **tmp,
		size_t *left)
{
	tlvdb->next = tlvdb->children = NULL;
	tlvdb->parent = parent;

	tlvdb->tag.tag = tlv_parse_tag(tmp, left);
	if (tlvdb->tag.tag == TLV_TAG_INVALID)
		goto err;

	tlvdb->tag.len = tlv_parse_len(tmp, left);
	if (tlvdb->tag.len == TLV_LEN_INVALID)
		goto err;

	if (tlvdb->tag.len > *left)
		goto err;

	tlvdb->tag.value = *tmp;

	*tmp += tlvdb->tag.len;
	*left -= tlvdb->tag.len;

	if (tlv_is_constructed(&tlvdb->tag) && (tlvdb->tag.len != 0)) {
		tlvdb->children = tlvdb_parse_children(tlvdb);
		if (!tlvdb->children)
			goto err;
	} else {
		tlvdb->children = NULL;
	}

	return true;

err:
	return false;
}

static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent)
{
	const unsigned char *tmp = parent->tag.value;
	size_t left = parent->tag.len;
	struct tlvdb *tlvdb, *first = NULL, *prev = NULL;

	while (left != 0) {
		tlvdb = malloc(sizeof(*tlvdb));
		if (prev)
			prev->next = tlvdb;
		else
			first = tlvdb;
		prev = tlvdb;

		if (!tlvdb_parse_one(tlvdb, parent, &tmp, &left))
			goto err;

		tlvdb->parent = parent;
	}

	return first;

err:
	tlvdb_free(first);

	return NULL;
}

struct tlvdb *tlvdb_parse(const unsigned char *buf, size_t len)
{
	struct tlvdb_root *root;
	const unsigned char *tmp;
	size_t left;

	if (!len || !buf)
		return NULL;

	root = malloc(sizeof(*root) + len);
	root->len = len;
	memcpy(root->buf, buf, len);

	tmp = root->buf;
	left = len;

	if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
		goto err;

	if (left)
		goto err;

	return &root->db;

err:
	tlvdb_free(&root->db);

	return NULL;
}

struct tlvdb *tlvdb_parse_multi(const unsigned char *buf, size_t len)
{
	struct tlvdb_root *root;
	const unsigned char *tmp;
	size_t left;

	if (!len || !buf)
		return NULL;

	root = malloc(sizeof(*root) + len);
	root->len = len;
	memcpy(root->buf, buf, len);

	tmp = root->buf;
	left = len;

	if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
		goto err;

	while (left != 0) {
		struct tlvdb *db = malloc(sizeof(*db));
		if (!tlvdb_parse_one(db, NULL, &tmp, &left)) {
			free (db);
			goto err;
		}

		tlvdb_add(&root->db, db);
	}

	return &root->db;

err:
	tlvdb_free(&root->db);

	return NULL;
}

struct tlvdb *tlvdb_fixed(tlv_tag_t tag, size_t len, const unsigned char *value)
{
	struct tlvdb_root *root = malloc(sizeof(*root) + len);

	root->len = len;
	memcpy(root->buf, value, len);

	root->db.parent = root->db.next = root->db.children = NULL;
	root->db.tag.tag = tag;
	root->db.tag.len = len;
	root->db.tag.value = root->buf;

	return &root->db;
}

struct tlvdb *tlvdb_external(tlv_tag_t tag, size_t len, const unsigned char *value)
{
	struct tlvdb_root *root = malloc(sizeof(*root));

	root->len = 0;

	root->db.parent = root->db.next = root->db.children = NULL;
	root->db.tag.tag = tag;
	root->db.tag.len = len;
	root->db.tag.value = value;

	return &root->db;
}

void tlvdb_free(struct tlvdb *tlvdb)
{
	struct tlvdb *next = NULL;

	if (!tlvdb)
		return;

	for (; tlvdb; tlvdb = next) {
		next = tlvdb->next;
		tlvdb_free(tlvdb->children);
		free(tlvdb);
	}
}

struct tlvdb *tlvdb_find_next(struct tlvdb *tlvdb, tlv_tag_t tag) {
	if (!tlvdb)
		return NULL;
	
	return tlvdb_find(tlvdb->next, tag);
}

struct tlvdb *tlvdb_find(struct tlvdb *tlvdb, tlv_tag_t tag) {
	if (!tlvdb)
		return NULL;
	
	for (; tlvdb; tlvdb = tlvdb->next) {
		if (tlvdb->tag.tag == tag)
			return tlvdb;
	}

	return NULL;
}

struct tlvdb *tlvdb_find_full(struct tlvdb *tlvdb, tlv_tag_t tag) {
	if (!tlvdb)
		return NULL;
	
	for (; tlvdb; tlvdb = tlvdb->next) {
		if (tlvdb->tag.tag == tag)
			return tlvdb;
		
		if (tlvdb->children) {
			struct tlvdb * ch = tlvdb_find_full(tlvdb->children, tag);
			if (ch)
				return ch;
		}			
	}

	return NULL;
}

struct tlvdb *tlvdb_find_path(struct tlvdb *tlvdb, tlv_tag_t tag[]) {
	int i = 0;
	struct tlvdb *tnext = tlvdb;
	
	while (tnext && tag[i]) {
		tnext = tlvdb_find(tnext, tag[i]);
		i++;
		if (tag[i] && tnext) {
			tnext = tnext->children;
		}
	}
	
	return tnext;
}

void tlvdb_add(struct tlvdb *tlvdb, struct tlvdb *other)
{
	while (tlvdb->next) {
		tlvdb = tlvdb->next;
	}

	tlvdb->next = other;
}

void tlvdb_change_or_add_node_ex(struct tlvdb *tlvdb, tlv_tag_t tag, size_t len, const unsigned char *value, struct tlvdb **tlvdb_elm)
{
	struct tlvdb *telm = tlvdb_find_full(tlvdb, tag);
	if (telm == NULL) {
		// new tlv element
		struct tlvdb *elm = tlvdb_fixed(tag, len, value);
		tlvdb_add(tlvdb, elm);
		if (tlvdb_elm)
			*tlvdb_elm = elm;
	} else {
		// the same tlv structure
		if (telm->tag.tag == tag && telm->tag.len == len && !memcmp(telm->tag.value, value, len))
			return;

		// replace tlv element
		struct tlvdb *tnewelm = tlvdb_fixed(tag, len, value);
		tnewelm->next = telm->next;
		tnewelm->parent = telm->parent;
		
		// if telm stayed first in children chain
		if (telm->parent && telm->parent->children == telm) {
			telm->parent->children = tnewelm;
		}
		
		// if telm have previous element
		if (telm != tlvdb) {
			// elm in root
			struct tlvdb *celm = tlvdb;
			// elm in child list of node
			if (telm->parent && telm->parent->children)
				celm = telm->parent->children;		
			
			// find previous element
			for (; celm; celm = celm->next) {
				if (celm->next == telm) {
					celm->next = tnewelm;
					break;
				}
			}
		}
		
		// free old element with childrens
		telm->next = NULL;
		tlvdb_free(telm);
		
		if (tlvdb_elm)
			*tlvdb_elm = tnewelm;
	}
	
	return;
}

void tlvdb_change_or_add_node(struct tlvdb *tlvdb, tlv_tag_t tag, size_t len, const unsigned char *value)
{
	tlvdb_change_or_add_node_ex(tlvdb, tag, len, value, NULL);
}

void tlvdb_visit(const struct tlvdb *tlvdb, tlv_cb cb, void *data, int level)
{
	struct tlvdb *next = NULL;

	if (!tlvdb)
		return;

	for (; tlvdb; tlvdb = next) {
		next = tlvdb->next;
		cb(data, &tlvdb->tag, level, (tlvdb->children == NULL));
		tlvdb_visit(tlvdb->children, cb, data, level + 1);
	}
}

static const struct tlvdb *tlvdb_next(const struct tlvdb *tlvdb)
{
	if (tlvdb->children)
		return tlvdb->children;

	while (tlvdb) {
		if (tlvdb->next)
			return tlvdb->next;

		tlvdb = tlvdb->parent;
	}

	return NULL;
}

const struct tlv *tlvdb_get(const struct tlvdb *tlvdb, tlv_tag_t tag, const struct tlv *prev)
{
	if (prev) {
//		tlvdb = tlvdb_next(container_of(prev, struct tlvdb, tag));
		tlvdb = tlvdb_next((struct tlvdb *)prev);
	}


	while (tlvdb) {
		if (tlvdb->tag.tag == tag)
			return &tlvdb->tag;

		tlvdb = tlvdb_next(tlvdb);
	}

	return NULL;
}

const struct tlv *tlvdb_get_inchild(const struct tlvdb *tlvdb, tlv_tag_t tag, const struct tlv *prev) {
	tlvdb = tlvdb->children;
	return tlvdb_get(tlvdb, tag, prev);
}

const struct tlv *tlvdb_get_tlv(const struct tlvdb *tlvdb) {
	return &tlvdb->tag;
}

unsigned char *tlv_encode(const struct tlv *tlv, size_t *len)
{
	size_t size = tlv->len;
	unsigned char *data;
	size_t pos;

	if (tlv->tag > 0x100)
		size += 2;
	else
		size += 1;

	if (tlv->len > 0x7f)
		size += 2;
	else
		size += 1;

	data = malloc(size);
	if (!data) {
		*len = 0;
		return NULL;
	}

	pos = 0;

	if (tlv->tag > 0x100) {
		data[pos++] = tlv->tag >> 8;
		data[pos++] = tlv->tag & 0xff;
	} else
		data[pos++] = tlv->tag;

	if (tlv->len > 0x7f) {
		data[pos++] = 0x81;
		data[pos++] = tlv->len;
	} else
		data[pos++] = tlv->len;

	memcpy(data + pos, tlv->value, tlv->len);
	pos += tlv->len;

	*len = pos;
	return data;
}

bool tlv_is_constructed(const struct tlv *tlv)
{
	return (tlv->tag < 0x100 ? tlv->tag : tlv->tag >> 8) & TLV_TAG_COMPLEX;
}

bool tlv_equal(const struct tlv *a, const struct tlv *b)
{
	if (!a && !b)
		return true;

	if (!a || !b)
		return false;

	return a->tag == b->tag && a->len == b->len && !memcmp(a->value, b->value, a->len);
}

struct tlvdb *tlvdb_elm_get_next(struct tlvdb *tlvdb)
{
	return tlvdb->next;
}

struct tlvdb *tlvdb_elm_get_children(struct tlvdb *tlvdb)
{
	return tlvdb->children;
}

struct tlvdb *tlvdb_elm_get_parent(struct tlvdb *tlvdb)
{
	return tlvdb->parent;
}

bool tlv_get_uint8(const struct tlv *etlv, uint8_t *value) 
{
	*value = 0;
	if (etlv)
	{
		if (etlv->len == 0)
			return true;
		
		if (etlv->len == 1)
		{
			*value = etlv->value[0];
			return true;
		}
	}
	return false;
}

bool tlv_get_int(const struct tlv *etlv, int *value)
{
	*value = 0;
	if (etlv)
	{
		if (etlv->len == 0)
			return true;
		
		if (etlv->len <= 4)
		{
			for (int i = 0; i < etlv->len; i++)
			{
				*value += etlv->value[i] * (1 << (i * 8));
			}
			return true;
		}
	}
	return false;
}
Commit	Line	Data
a2bb2735	1	/*
	2	* libopenemv - a library to work with EMV family of smart cards
	3	* Copyright (C) 2012, 2015 Dmitry Eremin-Solenikov
	4	*
	5	* This library is free software; you can redistribute it and/or
	6	* modify it under the terms of the GNU Lesser General Public
	7	* License as published by the Free Software Foundation; either
	8	* version 2.1 of the License, or (at your option) any later version.
	9	*
	10	* This library is distributed in the hope that it will be useful,
	11	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	12	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
	13	* Lesser General Public License for more details.
	14	*
	15	* https://github.com/lumag/emv-tools/blob/master/lib/tlv.c
	16	*/
	17
	18	#ifdef HAVE_CONFIG_H
	19	#include <config.h>
	20	#endif
	21
	22	#include "tlv.h"
	23
	24	#include <string.h>
	25	#include <stdint.h>
	26	#include <stddef.h>
	27	#include <stdlib.h>
	28
	29	#define TLV_TAG_CLASS_MASK 0xc0
	30	#define TLV_TAG_COMPLEX 0x20
	31	#define TLV_TAG_VALUE_MASK 0x1f
	32	#define TLV_TAG_VALUE_CONT 0x1f
	33	#define TLV_TAG_INVALID 0
	34
	35	#define TLV_LEN_LONG 0x80
	36	#define TLV_LEN_MASK 0x7f
	37	#define TLV_LEN_INVALID (~0)
	38
	39	// http://radek.io/2012/11/10/magical-container_of-macro/
	40	//#define container_of(ptr, type, member) ({
	41	// const typeof( ((type )0)->member ) __mptr = (ptr);
	42	// (type )( (char )__mptr - offsetof(type,member) );})
	43
	44	struct tlvdb {
	45	struct tlv tag;
	46	struct tlvdb *next;
	47	struct tlvdb *parent;
	48	struct tlvdb *children;
	49	};
	50
	51	struct tlvdb_root {
	52	struct tlvdb db;
	53	size_t len;
	54	unsigned char buf[0];
	55	};
	56
	57	static tlv_tag_t tlv_parse_tag(const unsigned char *buf, size_t len)
	58	{
	59	tlv_tag_t tag;
	60
	61	if (*len == 0)
	62	return TLV_TAG_INVALID;
	63	tag = **buf;
	64	--*len;
65	++*buf;
66	if ((tag & TLV_TAG_VALUE_MASK) != TLV_TAG_VALUE_CONT)
67	return tag;
68
69	if (*len == 0)
70	return TLV_TAG_INVALID;
71
72	tag <<= 8;
73	tag \|= **buf;
74	--*len;
75	++*buf;
76
77	return tag;
78	}
79
80	static size_t tlv_parse_len(const unsigned char *buf, size_t len)
81	{
82	size_t l;
83
84	if (*len == 0)
85	return TLV_LEN_INVALID;
86
87	l = **buf;
88	--*len;
89	++*buf;
90
91	if (!(l & TLV_LEN_LONG))
92	return l;
93
94	size_t ll = l &~ TLV_LEN_LONG;
14290308	95	if (ll > 5)
a2bb2735	96	return TLV_LEN_INVALID;
a2bb2735	97
14290308 OM	98	l = 0;
	99	for (int i = 1; i <= ll; i++) {
	100	l = (l << 8) + **buf;
	101	--*len;
	102	++*buf;
	103	}
a2bb2735	104
	105	return l;
	106	}
	107
	108	bool tlv_parse_tl(const unsigned char *buf, size_t len, struct tlv *tlv)
	109	{
	110	tlv->value = 0;
	111
	112	tlv->tag = tlv_parse_tag(buf, len);
	113	if (tlv->tag == TLV_TAG_INVALID)
	114	return false;
	115
	116	tlv->len = tlv_parse_len(buf, len);
	117	if (tlv->len == TLV_LEN_INVALID)
	118	return false;
	119
	120	return true;
	121	}
	122
	123	static struct tlvdb tlvdb_parse_children(struct tlvdb parent);
	124
	125	static bool tlvdb_parse_one(struct tlvdb *tlvdb,
	126	struct tlvdb *parent,
	127	const unsigned char **tmp,
	128	size_t *left)
	129	{
	130	tlvdb->next = tlvdb->children = NULL;
	131	tlvdb->parent = parent;
	132
	133	tlvdb->tag.tag = tlv_parse_tag(tmp, left);
	134	if (tlvdb->tag.tag == TLV_TAG_INVALID)
	135	goto err;
	136
	137	tlvdb->tag.len = tlv_parse_len(tmp, left);
	138	if (tlvdb->tag.len == TLV_LEN_INVALID)
	139	goto err;
	140
	141	if (tlvdb->tag.len > *left)
	142	goto err;
	143
	144	tlvdb->tag.value = *tmp;
	145
	146	*tmp += tlvdb->tag.len;
	147	*left -= tlvdb->tag.len;
	148
	149	if (tlv_is_constructed(&tlvdb->tag) && (tlvdb->tag.len != 0)) {
	150	tlvdb->children = tlvdb_parse_children(tlvdb);
	151	if (!tlvdb->children)
	152	goto err;
	153	} else {
	154	tlvdb->children = NULL;
	155	}
	156
	157	return true;
	158
	159	err:
	160	return false;
	161	}
	162
	163	static struct tlvdb tlvdb_parse_children(struct tlvdb parent)
	164	{
	165	const unsigned char *tmp = parent->tag.value;
	166	size_t left = parent->tag.len;
	167	struct tlvdb tlvdb, first = NULL, *prev = NULL;
168
169	while (left != 0) {
170	tlvdb = malloc(sizeof(*tlvdb));
171	if (prev)
172	prev->next = tlvdb;
173	else
174	first = tlvdb;
175	prev = tlvdb;
176
177	if (!tlvdb_parse_one(tlvdb, parent, &tmp, &left))
178	goto err;
179
180	tlvdb->parent = parent;
181	}
182
183	return first;
184
185	err:
186	tlvdb_free(first);
187
188	return NULL;
189	}
190
191	struct tlvdb tlvdb_parse(const unsigned char buf, size_t len)
192	{
193	struct tlvdb_root *root;
194	const unsigned char *tmp;
195	size_t left;
196
197	if (!len \|\| !buf)
198	return NULL;
199
200	root = malloc(sizeof(*root) + len);
201	root->len = len;
202	memcpy(root->buf, buf, len);
203
204	tmp = root->buf;
205	left = len;
206
207	if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
208	goto err;
209
210	if (left)
211	goto err;
212
213	return &root->db;
214
215	err:
216	tlvdb_free(&root->db);
217
218	return NULL;
219	}
220
221	struct tlvdb tlvdb_parse_multi(const unsigned char buf, size_t len)
222	{
223	struct tlvdb_root *root;
224	const unsigned char *tmp;
225	size_t left;
226
227	if (!len \|\| !buf)
228	return NULL;
229
230	root = malloc(sizeof(*root) + len);
231	root->len = len;
232	memcpy(root->buf, buf, len);
233
234	tmp = root->buf;
235	left = len;
236
237	if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
238	goto err;
239
240	while (left != 0) {
241	struct tlvdb db = malloc(sizeof(db));
242	if (!tlvdb_parse_one(db, NULL, &tmp, &left)) {
243	free (db);
244	goto err;
245	}
246
247	tlvdb_add(&root->db, db);
248	}
249
250	return &root->db;
251
252	err:
253	tlvdb_free(&root->db);
254
255	return NULL;
256	}
257
258	struct tlvdb tlvdb_fixed(tlv_tag_t tag, size_t len, const unsigned char value)
259	{
260	struct tlvdb_root root = malloc(sizeof(root) + len);
261
262	root->len = len;
263	memcpy(root->buf, value, len);
264
265	root->db.parent = root->db.next = root->db.children = NULL;
266	root->db.tag.tag = tag;
267	root->db.tag.len = len;
268	root->db.tag.value = root->buf;
269
270	return &root->db;
271	}
272
273	struct tlvdb tlvdb_external(tlv_tag_t tag, size_t len, const unsigned char value)
274	{
275	struct tlvdb_root root = malloc(sizeof(root));
276
277	root->len = 0;
278
279	root->db.parent = root->db.next = root->db.children = NULL;
280	root->db.tag.tag = tag;
281	root->db.tag.len = len;
282	root->db.tag.value = value;
283
284	return &root->db;
285	}
286
287	void tlvdb_free(struct tlvdb *tlvdb)
288	{
289	struct tlvdb *next = NULL;
290
291	if (!tlvdb)
292	return;
293
294	for (; tlvdb; tlvdb = next) {
295	next = tlvdb->next;
296	tlvdb_free(tlvdb->children);
297	free(tlvdb);
298	}
299	}
300
3c5fce2b OM	301	struct tlvdb tlvdb_find_next(struct tlvdb tlvdb, tlv_tag_t tag) {
	302	if (!tlvdb)
	303	return NULL;
	304
	305	return tlvdb_find(tlvdb->next, tag);
	306	}
	307
	308	struct tlvdb tlvdb_find(struct tlvdb tlvdb, tlv_tag_t tag) {
	309	if (!tlvdb)
	310	return NULL;
	311
	312	for (; tlvdb; tlvdb = tlvdb->next) {
	313	if (tlvdb->tag.tag == tag)
	314	return tlvdb;
	315	}
	316
	317	return NULL;
	318	}
	319
556826b5 OM	320	struct tlvdb tlvdb_find_full(struct tlvdb tlvdb, tlv_tag_t tag) {
	321	if (!tlvdb)
	322	return NULL;
	323
	324	for (; tlvdb; tlvdb = tlvdb->next) {
	325	if (tlvdb->tag.tag == tag)
	326	return tlvdb;
	327
	328	if (tlvdb->children) {
	329	struct tlvdb * ch = tlvdb_find_full(tlvdb->children, tag);
	330	if (ch)
	331	return ch;
	332	}
	333	}
	334
	335	return NULL;
	336	}
	337
3c5fce2b OM	338	struct tlvdb tlvdb_find_path(struct tlvdb tlvdb, tlv_tag_t tag[]) {
	339	int i = 0;
	340	struct tlvdb *tnext = tlvdb;
	341
	342	while (tnext && tag[i]) {
	343	tnext = tlvdb_find(tnext, tag[i]);
	344	i++;
	345	if (tag[i] && tnext) {
	346	tnext = tnext->children;
	347	}
	348	}
	349
	350	return tnext;
	351	}
	352
a2bb2735	353	void tlvdb_add(struct tlvdb tlvdb, struct tlvdb other)
	354	{
	355	while (tlvdb->next) {
	356	tlvdb = tlvdb->next;
	357	}
	358
	359	tlvdb->next = other;
	360	}
	361
4cdd63b2	362	void tlvdb_change_or_add_node_ex(struct tlvdb tlvdb, tlv_tag_t tag, size_t len, const unsigned char value, struct tlvdb **tlvdb_elm)
556826b5 OM	363	{
	364	struct tlvdb *telm = tlvdb_find_full(tlvdb, tag);
	365	if (telm == NULL) {
	366	// new tlv element
4cdd63b2	367	struct tlvdb *elm = tlvdb_fixed(tag, len, value);
	368	tlvdb_add(tlvdb, elm);
	369	if (tlvdb_elm)
	370	*tlvdb_elm = elm;
556826b5 OM	371	} else {
	372	// the same tlv structure
	373	if (telm->tag.tag == tag && telm->tag.len == len && !memcmp(telm->tag.value, value, len))
	374	return;
	375
	376	// replace tlv element
	377	struct tlvdb *tnewelm = tlvdb_fixed(tag, len, value);
	378	tnewelm->next = telm->next;
	379	tnewelm->parent = telm->parent;
	380
	381	// if telm stayed first in children chain
	382	if (telm->parent && telm->parent->children == telm) {
	383	telm->parent->children = tnewelm;
	384	}
	385
	386	// if telm have previous element
	387	if (telm != tlvdb) {
	388	// elm in root
	389	struct tlvdb *celm = tlvdb;
	390	// elm in child list of node
	391	if (telm->parent && telm->parent->children)
	392	celm = telm->parent->children;
	393
	394	// find previous element
	395	for (; celm; celm = celm->next) {
	396	if (celm->next == telm) {
	397	celm->next = tnewelm;
	398	break;
	399	}
	400	}
	401	}
	402
	403	// free old element with childrens
	404	telm->next = NULL;
	405	tlvdb_free(telm);
4cdd63b2	406
	407	if (tlvdb_elm)
	408	*tlvdb_elm = tnewelm;
556826b5 OM	409	}
	410
	411	return;
	412	}
	413
4cdd63b2	414	void tlvdb_change_or_add_node(struct tlvdb tlvdb, tlv_tag_t tag, size_t len, const unsigned char value)
	415	{
	416	tlvdb_change_or_add_node_ex(tlvdb, tag, len, value, NULL);
	417	}
	418
43912d63	419	void tlvdb_visit(const struct tlvdb tlvdb, tlv_cb cb, void data, int level)
a2bb2735	420	{
	421	struct tlvdb *next = NULL;
	422
	423	if (!tlvdb)
	424	return;
	425
	426	for (; tlvdb; tlvdb = next) {
	427	next = tlvdb->next;
3c5fce2b OM	428	cb(data, &tlvdb->tag, level, (tlvdb->children == NULL));
3c5fce2b OM	429	tlvdb_visit(tlvdb->children, cb, data, level + 1);
a2bb2735	430	}
	431	}
	432
	433	static const struct tlvdb tlvdb_next(const struct tlvdb tlvdb)
	434	{
	435	if (tlvdb->children)
	436	return tlvdb->children;
	437
	438	while (tlvdb) {
	439	if (tlvdb->next)
	440	return tlvdb->next;
	441
	442	tlvdb = tlvdb->parent;
	443	}
	444
	445	return NULL;
	446	}
	447
	448	const struct tlv tlvdb_get(const struct tlvdb tlvdb, tlv_tag_t tag, const struct tlv *prev)
	449	{
	450	if (prev) {
	451	// tlvdb = tlvdb_next(container_of(prev, struct tlvdb, tag));
	452	tlvdb = tlvdb_next((struct tlvdb *)prev);
	453	}
	454
	455
	456	while (tlvdb) {
	457	if (tlvdb->tag.tag == tag)
	458	return &tlvdb->tag;
	459
	460	tlvdb = tlvdb_next(tlvdb);
	461	}
	462
	463	return NULL;
	464	}
	465
3c5fce2b OM	466	const struct tlv tlvdb_get_inchild(const struct tlvdb tlvdb, tlv_tag_t tag, const struct tlv *prev) {
	467	tlvdb = tlvdb->children;
	468	return tlvdb_get(tlvdb, tag, prev);
	469	}
	470
95b697f0 OM	471	const struct tlv tlvdb_get_tlv(const struct tlvdb tlvdb) {
	472	return &tlvdb->tag;
	473	}
	474
a2bb2735	475	unsigned char tlv_encode(const struct tlv tlv, size_t *len)
	476	{
	477	size_t size = tlv->len;
	478	unsigned char *data;
	479	size_t pos;
	480
	481	if (tlv->tag > 0x100)
	482	size += 2;
	483	else
	484	size += 1;
	485
	486	if (tlv->len > 0x7f)
	487	size += 2;
	488	else
	489	size += 1;
	490
	491	data = malloc(size);
	492	if (!data) {
	493	*len = 0;
	494	return NULL;
	495	}
	496
	497	pos = 0;
	498
	499	if (tlv->tag > 0x100) {
	500	data[pos++] = tlv->tag >> 8;
	501	data[pos++] = tlv->tag & 0xff;
	502	} else
	503	data[pos++] = tlv->tag;
	504
	505	if (tlv->len > 0x7f) {
	506	data[pos++] = 0x81;
	507	data[pos++] = tlv->len;
	508	} else
	509	data[pos++] = tlv->len;
	510
	511	memcpy(data + pos, tlv->value, tlv->len);
	512	pos += tlv->len;
	513
	514	*len = pos;
	515	return data;
	516	}
	517
	518	bool tlv_is_constructed(const struct tlv *tlv)
	519	{
	520	return (tlv->tag < 0x100 ? tlv->tag : tlv->tag >> 8) & TLV_TAG_COMPLEX;
	521	}
	522
	523	bool tlv_equal(const struct tlv a, const struct tlv b)
	524	{
	525	if (!a && !b)
	526	return true;
	527
	528	if (!a \|\| !b)
	529	return false;
	530
	531	return a->tag == b->tag && a->len == b->len && !memcmp(a->value, b->value, a->len);
	532	}
95b697f0 OM	533
	534	struct tlvdb tlvdb_elm_get_next(struct tlvdb tlvdb)
	535	{
	536	return tlvdb->next;
	537	}
	538
	539	struct tlvdb tlvdb_elm_get_children(struct tlvdb tlvdb)
	540	{
	541	return tlvdb->children;
	542	}
	543
	544	struct tlvdb tlvdb_elm_get_parent(struct tlvdb tlvdb)
	545	{
	546	return tlvdb->parent;
	547	}
4cdd63b2	548
	549	bool tlv_get_uint8(const struct tlv etlv, uint8_t value)
	550	{
	551	*value = 0;
	552	if (etlv)
	553	{
	554	if (etlv->len == 0)
	555	return true;
	556
	557	if (etlv->len == 1)
	558	{
	559	*value = etlv->value[0];
	560	return true;
	561	}
	562	}
	563	return false;
	564	}
	565
	566	bool tlv_get_int(const struct tlv etlv, int value)
	567	{
	568	*value = 0;
	569	if (etlv)
	570	{
	571	if (etlv->len == 0)
	572	return true;
	573
	574	if (etlv->len <= 4)
	575	{
	576	for (int i = 0; i < etlv->len; i++)
	577	{
	578	value += etlv->value[i] (1 << (i * 8));
	579	}
	580	return true;
	581	}
	582	}
	583	return false;
	584	}