[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 (*len < ll)
		return TLV_LEN_INVALID;

	/* FIXME */
	if (ll != 1)
		return TLV_LEN_INVALID;

	l = **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);
	}
}

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

	tlvdb->next = other;
}

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;
		bool is_leaf = (tlvdb->children == NULL);
		cb(data, &tlvdb->tag, level, is_leaf);
		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;
}

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);
}
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;
95	if (*len < ll)
96	return TLV_LEN_INVALID;
97
98	/* FIXME */
99	if (ll != 1)
100	return TLV_LEN_INVALID;
101
102	l = **buf;
103	--*len;
104	++*buf;
105
106	return l;
107	}
108
109	bool tlv_parse_tl(const unsigned char *buf, size_t len, struct tlv *tlv)
110	{
111	tlv->value = 0;
112
113	tlv->tag = tlv_parse_tag(buf, len);
114	if (tlv->tag == TLV_TAG_INVALID)
115	return false;
116
117	tlv->len = tlv_parse_len(buf, len);
118	if (tlv->len == TLV_LEN_INVALID)
119	return false;
120
121	return true;
122	}
123
124	static struct tlvdb tlvdb_parse_children(struct tlvdb parent);
125
126	static bool tlvdb_parse_one(struct tlvdb *tlvdb,
127	struct tlvdb *parent,
128	const unsigned char **tmp,
129	size_t *left)
130	{
131	tlvdb->next = tlvdb->children = NULL;
132	tlvdb->parent = parent;
133
134	tlvdb->tag.tag = tlv_parse_tag(tmp, left);
135	if (tlvdb->tag.tag == TLV_TAG_INVALID)
136	goto err;
137
138	tlvdb->tag.len = tlv_parse_len(tmp, left);
139	if (tlvdb->tag.len == TLV_LEN_INVALID)
140	goto err;
141
142	if (tlvdb->tag.len > *left)
143	goto err;
144
145	tlvdb->tag.value = *tmp;
146
147	*tmp += tlvdb->tag.len;
148	*left -= tlvdb->tag.len;
149
150	if (tlv_is_constructed(&tlvdb->tag) && (tlvdb->tag.len != 0)) {
151	tlvdb->children = tlvdb_parse_children(tlvdb);
152	if (!tlvdb->children)
153	goto err;
154	} else {
155	tlvdb->children = NULL;
156	}
157
158	return true;
159
160	err:
161	return false;
162	}
163
164	static struct tlvdb tlvdb_parse_children(struct tlvdb parent)
165	{
166	const unsigned char *tmp = parent->tag.value;
167	size_t left = parent->tag.len;
168	struct tlvdb tlvdb, first = NULL, *prev = NULL;
169
170	while (left != 0) {
171	tlvdb = malloc(sizeof(*tlvdb));
172	if (prev)
173	prev->next = tlvdb;
174	else
175	first = tlvdb;
176	prev = tlvdb;
177
178	if (!tlvdb_parse_one(tlvdb, parent, &tmp, &left))
179	goto err;
180
181	tlvdb->parent = parent;
182	}
183
184	return first;
185
186	err:
187	tlvdb_free(first);
188
189	return NULL;
190	}
191
192	struct tlvdb tlvdb_parse(const unsigned char buf, size_t len)
193	{
194	struct tlvdb_root *root;
195	const unsigned char *tmp;
196	size_t left;
197
198	if (!len \|\| !buf)
199	return NULL;
200
201	root = malloc(sizeof(*root) + len);
202	root->len = len;
203	memcpy(root->buf, buf, len);
204
205	tmp = root->buf;
206	left = len;
207
208	if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
209	goto err;
210
211	if (left)
212	goto err;
213
214	return &root->db;
215
216	err:
217	tlvdb_free(&root->db);
218
219	return NULL;
220	}
221
222	struct tlvdb tlvdb_parse_multi(const unsigned char buf, size_t len)
223	{
224	struct tlvdb_root *root;
225	const unsigned char *tmp;
226	size_t left;
227
228	if (!len \|\| !buf)
229	return NULL;
230
231	root = malloc(sizeof(*root) + len);
232	root->len = len;
233	memcpy(root->buf, buf, len);
234
235	tmp = root->buf;
236	left = len;
237
238	if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
239	goto err;
240
241	while (left != 0) {
242	struct tlvdb db = malloc(sizeof(db));
243	if (!tlvdb_parse_one(db, NULL, &tmp, &left)) {
244	free (db);
245	goto err;
246	}
247
248	tlvdb_add(&root->db, db);
249	}
250
251	return &root->db;
252
253	err:
254	tlvdb_free(&root->db);
255
256	return NULL;
257	}
258
259	struct tlvdb tlvdb_fixed(tlv_tag_t tag, size_t len, const unsigned char value)
260	{
261	struct tlvdb_root root = malloc(sizeof(root) + len);
262
263	root->len = len;
264	memcpy(root->buf, value, len);
265
266	root->db.parent = root->db.next = root->db.children = NULL;
267	root->db.tag.tag = tag;
268	root->db.tag.len = len;
269	root->db.tag.value = root->buf;
270
271	return &root->db;
272	}
273
274	struct tlvdb tlvdb_external(tlv_tag_t tag, size_t len, const unsigned char value)
275	{
276	struct tlvdb_root root = malloc(sizeof(root));
277
278	root->len = 0;
279
280	root->db.parent = root->db.next = root->db.children = NULL;
281	root->db.tag.tag = tag;
282	root->db.tag.len = len;
283	root->db.tag.value = value;
284
285	return &root->db;
286	}
287
288	void tlvdb_free(struct tlvdb *tlvdb)
289	{
290	struct tlvdb *next = NULL;
291
292	if (!tlvdb)
293	return;
294
295	for (; tlvdb; tlvdb = next) {
296	next = tlvdb->next;
297	tlvdb_free(tlvdb->children);
298	free(tlvdb);
299	}
300	}
301
302	void tlvdb_add(struct tlvdb tlvdb, struct tlvdb other)
303	{
304	while (tlvdb->next) {
305	tlvdb = tlvdb->next;
306	}
307
308	tlvdb->next = other;
309	}
310
43912d63	311	void tlvdb_visit(const struct tlvdb tlvdb, tlv_cb cb, void data, int level)
a2bb2735	312	{
	313	struct tlvdb *next = NULL;
	314
	315	if (!tlvdb)
	316	return;
	317
	318	for (; tlvdb; tlvdb = next) {
	319	next = tlvdb->next;
33a9982c	320	bool is_leaf = (tlvdb->children == NULL);
33a9982c	321	cb(data, &tlvdb->tag, level, is_leaf);
43912d63	322	tlvdb_visit(tlvdb->children, cb, data, level+1);
a2bb2735	323	}
	324	}
	325
	326	static const struct tlvdb tlvdb_next(const struct tlvdb tlvdb)
	327	{
	328	if (tlvdb->children)
	329	return tlvdb->children;
	330
	331	while (tlvdb) {
	332	if (tlvdb->next)
	333	return tlvdb->next;
	334
	335	tlvdb = tlvdb->parent;
	336	}
	337
	338	return NULL;
	339	}
	340
	341	const struct tlv tlvdb_get(const struct tlvdb tlvdb, tlv_tag_t tag, const struct tlv *prev)
	342	{
	343	if (prev) {
	344	// tlvdb = tlvdb_next(container_of(prev, struct tlvdb, tag));
	345	tlvdb = tlvdb_next((struct tlvdb *)prev);
	346	}
	347
	348
	349	while (tlvdb) {
	350	if (tlvdb->tag.tag == tag)
	351	return &tlvdb->tag;
	352
	353	tlvdb = tlvdb_next(tlvdb);
	354	}
	355
	356	return NULL;
	357	}
	358
	359	unsigned char tlv_encode(const struct tlv tlv, size_t *len)
	360	{
	361	size_t size = tlv->len;
	362	unsigned char *data;
	363	size_t pos;
	364
	365	if (tlv->tag > 0x100)
	366	size += 2;
	367	else
	368	size += 1;
	369
	370	if (tlv->len > 0x7f)
	371	size += 2;
	372	else
	373	size += 1;
	374
	375	data = malloc(size);
	376	if (!data) {
	377	*len = 0;
	378	return NULL;
	379	}
	380
	381	pos = 0;
	382
	383	if (tlv->tag > 0x100) {
	384	data[pos++] = tlv->tag >> 8;
	385	data[pos++] = tlv->tag & 0xff;
	386	} else
387	data[pos++] = tlv->tag;
388
389	if (tlv->len > 0x7f) {
390	data[pos++] = 0x81;
391	data[pos++] = tlv->len;
392	} else
393	data[pos++] = tlv->len;
394
395	memcpy(data + pos, tlv->value, tlv->len);
396	pos += tlv->len;
397
398	*len = pos;
399	return data;
400	}
401
402	bool tlv_is_constructed(const struct tlv *tlv)
403	{
404	return (tlv->tag < 0x100 ? tlv->tag : tlv->tag >> 8) & TLV_TAG_COMPLEX;
405	}
406
407	bool tlv_equal(const struct tlv a, const struct tlv b)
408	{
409	if (!a && !b)
410	return true;
411
412	if (!a \|\| !b)
413	return false;
414
415	return a->tag == b->tag && a->len == b->len && !memcmp(a->value, b->value, a->len);
416	}