fix get length in tlv (#714)

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

diff --git a/client/emv/tlv.c b/client/emv/tlv.c
index 1be917776f20a7bc8c951ae60dd554e692b3e09e..35bdb5d4cca05d161fca1c54cef0b6c7afe862c0 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;
 }


@@ -318,6 +317,24 @@ struct tlvdb *tlvdb_find(struct tlvdb *tlvdb, tlv_tag_t tag) {
        return NULL;
 }
 
        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;
 struct tlvdb *tlvdb_find_path(struct tlvdb *tlvdb, tlv_tag_t tag[]) {
        int i = 0;
        struct tlvdb *tnext = tlvdb;


@@ -342,6 +359,52 @@ void tlvdb_add(struct tlvdb *tlvdb, struct tlvdb *other)
        tlvdb->next = other;
 }
 
        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;
 void tlvdb_visit(const struct tlvdb *tlvdb, tlv_cb cb, void *data, int level)
 {
        struct tlvdb *next = NULL;


@@ -394,6 +457,10 @@ const struct tlv *tlvdb_get_inchild(const struct tlvdb *tlvdb, tlv_tag_t tag, co
        return tlvdb_get(tlvdb, tag, prev);
 }
 
        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;


@@ -452,3 +519,18 @@ 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;
+}