diff --git a/client/emv/tlv.c b/client/emv/tlv.c index 0ec12364..08e09f23 100644 --- a/client/emv/tlv.c +++ b/client/emv/tlv.c @@ -318,6 +318,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; @@ -344,7 +362,7 @@ void tlvdb_add(struct tlvdb *tlvdb, struct tlvdb *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(tlvdb, tag); + struct tlvdb *telm = tlvdb_find_full(tlvdb, tag); if (telm == NULL) { // new tlv element tlvdb_add(tlvdb, tlvdb_fixed(tag, len, value)); @@ -353,19 +371,36 @@ void tlvdb_change_or_add_node(struct tlvdb *tlvdb, tlv_tag_t tag, size_t len, co if (telm->tag.tag == tag && telm->tag.len == len && !memcmp(telm->tag.value, value, len)) return; -PrintAndLog("-replace %x", tag); // replace tlv element struct tlvdb *tnewelm = tlvdb_fixed(tag, len, value); tnewelm->next = telm->next; tnewelm->parent = telm->parent; + // if elm stays 1st in children link if (telm->parent && telm->parent->children == telm) { telm->parent->children = tnewelm; } + // if elm have prev 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; + } + } + } -// telm->next = NULL; -// tlvdb_free(telm); + // free old element + telm->next = NULL; + tlvdb_free(telm); } return; diff --git a/client/emv/tlv.h b/client/emv/tlv.h index 295a3874..963abb2a 100644 --- a/client/emv/tlv.h +++ b/client/emv/tlv.h @@ -39,6 +39,7 @@ struct tlvdb *tlvdb_parse(const unsigned char *buf, size_t len); struct tlvdb *tlvdb_parse_multi(const unsigned char *buf, size_t len); void tlvdb_free(struct tlvdb *tlvdb); +struct tlvdb *tlvdb_find_full(struct tlvdb *tlvdb, tlv_tag_t tag); // search also in childrens struct tlvdb *tlvdb_find(struct tlvdb *tlvdb, tlv_tag_t tag); struct tlvdb *tlvdb_find_next(struct tlvdb *tlvdb, tlv_tag_t tag); struct tlvdb *tlvdb_find_path(struct tlvdb *tlvdb, tlv_tag_t tag[]);