X-Git-Url: https://git.zerfleddert.de/cgi-bin/gitweb.cgi/proxmark3-svn/blobdiff_plain/653e5ed3ca3f8bf4ae677900ae4860a8cf7026c1..f98702bace48fde3ec1492d1c732e23cd2eb8613:/client/emv/tlv.c?ds=sidebyside diff --git a/client/emv/tlv.c b/client/emv/tlv.c index 1be91777..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; } @@ -318,6 +317,24 @@ struct tlvdb *tlvdb_find(struct tlvdb *tlvdb, tlv_tag_t tag) { 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; @@ -335,13 +352,76 @@ struct tlvdb *tlvdb_find_path(struct tlvdb *tlvdb, tlv_tag_t tag[]) { 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_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; @@ -394,6 +474,10 @@ const struct tlv *tlvdb_get_inchild(const struct tlvdb *tlvdb, tlv_tag_t tag, co 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; @@ -452,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); +}