X-Git-Url: https://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/e464f6225860a0965e266732328d9d0948607c31..refs/pull/943/head:/client/emv/tlv.c

diff --git a/client/emv/tlv.c b/client/emv/tlv.c
index d78f049e..5472c47a 100644
--- a/client/emv/tlv.c
+++ b/client/emv/tlv.c
@@ -92,16 +92,15 @@ static size_t tlv_parse_len(const unsigned char **buf, size_t *len)
 		return l;
 
 	size_t ll = l &~ TLV_LEN_LONG;
-	if (*len < ll)
+	if (ll > 5)
 		return TLV_LEN_INVALID;
 
-	/* FIXME */
-	if (ll != 1)
-		return TLV_LEN_INVALID;
-
-	l = **buf;
-	--*len;
-	++*buf;
+	l = 0;
+	for (int i = 1; i <= ll; i++) {
+		l = (l << 8) + **buf;
+		--*len;
+		++*buf;
+	}
 
 	return l;
 }
@@ -299,16 +298,131 @@ void tlvdb_free(struct tlvdb *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_visit(const struct tlvdb *tlvdb, tlv_cb cb, void *data)
+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;
 
@@ -317,8 +431,8 @@ void tlvdb_visit(const struct tlvdb *tlvdb, tlv_cb cb, void *data)
 
 	for (; tlvdb; tlvdb = next) {
 		next = tlvdb->next;
-		cb(data, &tlvdb->tag);
-		tlvdb_visit(tlvdb->children, cb, data);
+		cb(data, &tlvdb->tag, level, (tlvdb->children == NULL));
+		tlvdb_visit(tlvdb->children, cb, data, level + 1);
 	}
 }
 
@@ -355,6 +469,15 @@ const struct tlv *tlvdb_get(const struct tlvdb *tlvdb, tlv_tag_t tag, const stru
 	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;
@@ -413,3 +536,61 @@ bool tlv_equal(const struct tlv *a, const struct tlv *b)
 
 	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);
+}