[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)
{
	if (tlvdb == other)
		return;
	
	while (tlvdb->next) {
		if (tlvdb->next == other)
			return;
		
		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;
}

bool tlvdb_get_uint8(struct tlvdb *tlvRoot, tlv_tag_t tag, uint8_t *value)
{
	const struct tlv *tlvelm = tlvdb_get(tlvRoot, tag, NULL); 	
	return tlv_get_uint8(tlvelm, value);
}
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)
a2bb2735	354	{
7527c2bd OM	355	if (tlvdb == other)
	356	return;
	357
a2bb2735	358	while (tlvdb->next) {
7527c2bd OM	359	if (tlvdb->next == other)
	360	return;
	361
a2bb2735	362	tlvdb = tlvdb->next;
	363	}
	364
	365	tlvdb->next = other;
	366	}
	367
4cdd63b2	368	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	369	{
	370	struct tlvdb *telm = tlvdb_find_full(tlvdb, tag);
	371	if (telm == NULL) {
	372	// new tlv element
4cdd63b2	373	struct tlvdb *elm = tlvdb_fixed(tag, len, value);
	374	tlvdb_add(tlvdb, elm);
	375	if (tlvdb_elm)
	376	*tlvdb_elm = elm;
556826b5 OM	377	} else {
	378	// the same tlv structure
	379	if (telm->tag.tag == tag && telm->tag.len == len && !memcmp(telm->tag.value, value, len))
	380	return;
	381
	382	// replace tlv element
	383	struct tlvdb *tnewelm = tlvdb_fixed(tag, len, value);
	384	tnewelm->next = telm->next;
	385	tnewelm->parent = telm->parent;
	386
	387	// if telm stayed first in children chain
	388	if (telm->parent && telm->parent->children == telm) {
	389	telm->parent->children = tnewelm;
	390	}
	391
	392	// if telm have previous element
	393	if (telm != tlvdb) {
	394	// elm in root
	395	struct tlvdb *celm = tlvdb;
	396	// elm in child list of node
	397	if (telm->parent && telm->parent->children)
	398	celm = telm->parent->children;
	399
	400	// find previous element
	401	for (; celm; celm = celm->next) {
	402	if (celm->next == telm) {
	403	celm->next = tnewelm;
	404	break;
	405	}
	406	}
	407	}
	408
	409	// free old element with childrens
	410	telm->next = NULL;
	411	tlvdb_free(telm);
4cdd63b2	412
	413	if (tlvdb_elm)
	414	*tlvdb_elm = tnewelm;
556826b5 OM	415	}
	416
	417	return;
	418	}
	419
4cdd63b2	420	void tlvdb_change_or_add_node(struct tlvdb tlvdb, tlv_tag_t tag, size_t len, const unsigned char value)
	421	{
	422	tlvdb_change_or_add_node_ex(tlvdb, tag, len, value, NULL);
	423	}
	424
43912d63	425	void tlvdb_visit(const struct tlvdb tlvdb, tlv_cb cb, void data, int level)
a2bb2735	426	{
	427	struct tlvdb *next = NULL;
	428
	429	if (!tlvdb)
	430	return;
	431
	432	for (; tlvdb; tlvdb = next) {
	433	next = tlvdb->next;
3c5fce2b OM	434	cb(data, &tlvdb->tag, level, (tlvdb->children == NULL));
3c5fce2b OM	435	tlvdb_visit(tlvdb->children, cb, data, level + 1);
a2bb2735	436	}
	437	}
	438
	439	static const struct tlvdb tlvdb_next(const struct tlvdb tlvdb)
	440	{
	441	if (tlvdb->children)
	442	return tlvdb->children;
	443
	444	while (tlvdb) {
	445	if (tlvdb->next)
	446	return tlvdb->next;
	447
	448	tlvdb = tlvdb->parent;
	449	}
	450
	451	return NULL;
	452	}
	453
	454	const struct tlv tlvdb_get(const struct tlvdb tlvdb, tlv_tag_t tag, const struct tlv *prev)
	455	{
	456	if (prev) {
	457	// tlvdb = tlvdb_next(container_of(prev, struct tlvdb, tag));
	458	tlvdb = tlvdb_next((struct tlvdb *)prev);
	459	}
	460
	461
	462	while (tlvdb) {
	463	if (tlvdb->tag.tag == tag)
	464	return &tlvdb->tag;
	465
	466	tlvdb = tlvdb_next(tlvdb);
	467	}
	468
	469	return NULL;
	470	}
	471
3c5fce2b OM	472	const struct tlv tlvdb_get_inchild(const struct tlvdb tlvdb, tlv_tag_t tag, const struct tlv *prev) {
	473	tlvdb = tlvdb->children;
	474	return tlvdb_get(tlvdb, tag, prev);
	475	}
	476
95b697f0 OM	477	const struct tlv tlvdb_get_tlv(const struct tlvdb tlvdb) {
	478	return &tlvdb->tag;
	479	}
	480
a2bb2735	481	unsigned char tlv_encode(const struct tlv tlv, size_t *len)
	482	{
	483	size_t size = tlv->len;
	484	unsigned char *data;
	485	size_t pos;
	486
	487	if (tlv->tag > 0x100)
	488	size += 2;
	489	else
	490	size += 1;
	491
	492	if (tlv->len > 0x7f)
	493	size += 2;
	494	else
	495	size += 1;
	496
	497	data = malloc(size);
	498	if (!data) {
	499	*len = 0;
	500	return NULL;
	501	}
	502
	503	pos = 0;
	504
	505	if (tlv->tag > 0x100) {
	506	data[pos++] = tlv->tag >> 8;
	507	data[pos++] = tlv->tag & 0xff;
	508	} else
	509	data[pos++] = tlv->tag;
	510
	511	if (tlv->len > 0x7f) {
	512	data[pos++] = 0x81;
	513	data[pos++] = tlv->len;
	514	} else
	515	data[pos++] = tlv->len;
	516
	517	memcpy(data + pos, tlv->value, tlv->len);
	518	pos += tlv->len;
	519
	520	*len = pos;
	521	return data;
	522	}
	523
	524	bool tlv_is_constructed(const struct tlv *tlv)
	525	{
	526	return (tlv->tag < 0x100 ? tlv->tag : tlv->tag >> 8) & TLV_TAG_COMPLEX;
	527	}
	528
	529	bool tlv_equal(const struct tlv a, const struct tlv b)
	530	{
	531	if (!a && !b)
	532	return true;
	533
	534	if (!a \|\| !b)
	535	return false;
	536
	537	return a->tag == b->tag && a->len == b->len && !memcmp(a->value, b->value, a->len);
	538	}
95b697f0 OM	539
	540	struct tlvdb tlvdb_elm_get_next(struct tlvdb tlvdb)
	541	{
	542	return tlvdb->next;
	543	}
	544
	545	struct tlvdb tlvdb_elm_get_children(struct tlvdb tlvdb)
	546	{
	547	return tlvdb->children;
	548	}
	549
	550	struct tlvdb tlvdb_elm_get_parent(struct tlvdb tlvdb)
	551	{
	552	return tlvdb->parent;
	553	}
4cdd63b2	554
	555	bool tlv_get_uint8(const struct tlv etlv, uint8_t value)
	556	{
	557	*value = 0;
	558	if (etlv)
	559	{
	560	if (etlv->len == 0)
	561	return true;
	562
	563	if (etlv->len == 1)
	564	{
	565	*value = etlv->value[0];
	566	return true;
	567	}
	568	}
	569	return false;
	570	}
	571
	572	bool tlv_get_int(const struct tlv etlv, int value)
	573	{
	574	*value = 0;
	575	if (etlv)
	576	{
	577	if (etlv->len == 0)
	578	return true;
	579
	580	if (etlv->len <= 4)
	581	{
	582	for (int i = 0; i < etlv->len; i++)
	583	{
	584	value += etlv->value[i] (1 << (i * 8));
	585	}
	586	return true;
	587	}
	588	}
	589	return false;
	590	}
0b6efd01 OM	591
	592	bool tlvdb_get_uint8(struct tlvdb tlvRoot, tlv_tag_t tag, uint8_t value)
	593	{
	594	const struct tlv *tlvelm = tlvdb_get(tlvRoot, tag, NULL);
	595	return tlv_get_uint8(tlvelm, value);
	596	}