fix 'hf iclass sim'

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

diff --git a/client/emv/tlv.c b/client/emv/tlv.c
index d78f049e07bb7577cf7265fc0d12707420e01926..5472c47ad82e8debfe51190fed2cd227b5349ca1 100644 (file)
--- 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;
                return l;
 
        size_t ll = l &~ TLV_LEN_LONG;

-       if (*len < ll)
+       if (ll > 5)

                return TLV_LEN_INVALID;
 
                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;
 }
 
        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)
 {
 void tlvdb_add(struct tlvdb *tlvdb, struct tlvdb *other)
 {

+       if (tlvdb == other)
+               return;
+       

        while (tlvdb->next) {
        while (tlvdb->next) {

+               if (tlvdb->next == other)
+                       return;
+               

                tlvdb = tlvdb->next;
        }
 
        tlvdb->next = other;
 }
 
                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;
 
 {
        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;
 
        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;
 }
 
        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 *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);
 }
 
        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);
+}