[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(struct tlvdb *tlvdb, tlv_tag_t tag, size_t len, const unsigned char *value)
{
	struct tlvdb *telm = tlvdb_find_full(tlvdb, tag);
	if (telm == NULL) {
		// new tlv element
		tlvdb_add(tlvdb, tlvdb_fixed(tag, len, value));
	} 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);
	}
	
	return;
}

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;
}
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
556826b5 OM	362	void tlvdb_change_or_add_node(struct tlvdb tlvdb, tlv_tag_t tag, size_t len, const unsigned char value)
	363	{
	364	struct tlvdb *telm = tlvdb_find_full(tlvdb, tag);
	365	if (telm == NULL) {
	366	// new tlv element
	367	tlvdb_add(tlvdb, tlvdb_fixed(tag, len, value));
	368	} else {
	369	// the same tlv structure
	370	if (telm->tag.tag == tag && telm->tag.len == len && !memcmp(telm->tag.value, value, len))
	371	return;
	372
	373	// replace tlv element
	374	struct tlvdb *tnewelm = tlvdb_fixed(tag, len, value);
	375	tnewelm->next = telm->next;
	376	tnewelm->parent = telm->parent;
	377
	378	// if telm stayed first in children chain
	379	if (telm->parent && telm->parent->children == telm) {
	380	telm->parent->children = tnewelm;
	381	}
	382
	383	// if telm have previous element
	384	if (telm != tlvdb) {
	385	// elm in root
	386	struct tlvdb *celm = tlvdb;
	387	// elm in child list of node
	388	if (telm->parent && telm->parent->children)
	389	celm = telm->parent->children;
	390
	391	// find previous element
	392	for (; celm; celm = celm->next) {
	393	if (celm->next == telm) {
	394	celm->next = tnewelm;
	395	break;
	396	}
	397	}
	398	}
	399
	400	// free old element with childrens
	401	telm->next = NULL;
	402	tlvdb_free(telm);
	403	}
	404
	405	return;
	406	}
	407
43912d63	408	void tlvdb_visit(const struct tlvdb tlvdb, tlv_cb cb, void data, int level)
a2bb2735	409	{
	410	struct tlvdb *next = NULL;
	411
	412	if (!tlvdb)
	413	return;
	414
	415	for (; tlvdb; tlvdb = next) {
	416	next = tlvdb->next;
3c5fce2b OM	417	cb(data, &tlvdb->tag, level, (tlvdb->children == NULL));
3c5fce2b OM	418	tlvdb_visit(tlvdb->children, cb, data, level + 1);
a2bb2735	419	}
	420	}
	421
	422	static const struct tlvdb tlvdb_next(const struct tlvdb tlvdb)
	423	{
	424	if (tlvdb->children)
	425	return tlvdb->children;
	426
	427	while (tlvdb) {
	428	if (tlvdb->next)
	429	return tlvdb->next;
	430
	431	tlvdb = tlvdb->parent;
	432	}
	433
	434	return NULL;
	435	}
	436
	437	const struct tlv tlvdb_get(const struct tlvdb tlvdb, tlv_tag_t tag, const struct tlv *prev)
	438	{
	439	if (prev) {
	440	// tlvdb = tlvdb_next(container_of(prev, struct tlvdb, tag));
	441	tlvdb = tlvdb_next((struct tlvdb *)prev);
	442	}
	443
	444
	445	while (tlvdb) {
	446	if (tlvdb->tag.tag == tag)
	447	return &tlvdb->tag;
	448
	449	tlvdb = tlvdb_next(tlvdb);
	450	}
	451
	452	return NULL;
	453	}
	454
3c5fce2b OM	455	const struct tlv tlvdb_get_inchild(const struct tlvdb tlvdb, tlv_tag_t tag, const struct tlv *prev) {
	456	tlvdb = tlvdb->children;
	457	return tlvdb_get(tlvdb, tag, prev);
	458	}
	459
95b697f0 OM	460	const struct tlv tlvdb_get_tlv(const struct tlvdb tlvdb) {
	461	return &tlvdb->tag;
	462	}
	463
a2bb2735	464	unsigned char tlv_encode(const struct tlv tlv, size_t *len)
	465	{
	466	size_t size = tlv->len;
	467	unsigned char *data;
	468	size_t pos;
	469
	470	if (tlv->tag > 0x100)
	471	size += 2;
	472	else
	473	size += 1;
	474
	475	if (tlv->len > 0x7f)
	476	size += 2;
	477	else
	478	size += 1;
	479
	480	data = malloc(size);
	481	if (!data) {
	482	*len = 0;
	483	return NULL;
	484	}
	485
	486	pos = 0;
	487
	488	if (tlv->tag > 0x100) {
	489	data[pos++] = tlv->tag >> 8;
	490	data[pos++] = tlv->tag & 0xff;
	491	} else
	492	data[pos++] = tlv->tag;
	493
	494	if (tlv->len > 0x7f) {
	495	data[pos++] = 0x81;
	496	data[pos++] = tlv->len;
	497	} else
	498	data[pos++] = tlv->len;
	499
	500	memcpy(data + pos, tlv->value, tlv->len);
	501	pos += tlv->len;
	502
	503	*len = pos;
	504	return data;
	505	}
	506
	507	bool tlv_is_constructed(const struct tlv *tlv)
	508	{
	509	return (tlv->tag < 0x100 ? tlv->tag : tlv->tag >> 8) & TLV_TAG_COMPLEX;
	510	}
	511
	512	bool tlv_equal(const struct tlv a, const struct tlv b)
	513	{
	514	if (!a && !b)
	515	return true;
	516
	517	if (!a \|\| !b)
	518	return false;
	519
	520	return a->tag == b->tag && a->len == b->len && !memcmp(a->value, b->value, a->len);
	521	}
95b697f0 OM	522
	523	struct tlvdb tlvdb_elm_get_next(struct tlvdb tlvdb)
	524	{
	525	return tlvdb->next;
	526	}
	527
	528	struct tlvdb tlvdb_elm_get_children(struct tlvdb tlvdb)
	529	{
	530	return tlvdb->children;
	531	}
	532
	533	struct tlvdb tlvdb_elm_get_parent(struct tlvdb tlvdb)
	534	{
	535	return tlvdb->parent;
	536	}