proxmark3/client/emv/tlv.c

576 lines
13 KiB
C
Raw Normal View History

2017-11-11 03:08:28 +08:00
/*
* libopenemv - a library to work with EMV family of smart cards
* Copyright (C) 2012, 2015 Dmitry Eremin-Solenikov
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
2017-11-11 03:08:28 +08:00
* https://github.com/lumag/emv-tools/blob/master/lib/tlv.c
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include "tlv.h"
#include <string.h>
#include <stdlib.h>
2019-03-10 06:35:06 +08:00
#define TLV_TAG_CLASS_MASK 0xc0
#define TLV_TAG_COMPLEX 0x20
#define TLV_TAG_VALUE_MASK 0x1f
#define TLV_TAG_VALUE_CONT 0x1f
#define TLV_TAG_INVALID 0
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
#define TLV_LEN_LONG 0x80
#define TLV_LEN_MASK 0x7f
#define TLV_LEN_INVALID (~0)
2017-11-11 03:08:28 +08:00
// http://radek.io/2012/11/10/magical-container_of-macro/
//#define container_of(ptr, type, member) ({
2019-03-10 06:35:06 +08:00
// const typeof( ((type *)0)->member ) *__mptr = (ptr);
2017-11-11 03:08:28 +08:00
// (type *)( (char *)__mptr - offsetof(type,member) );})
struct tlvdb {
2019-03-10 06:35:06 +08:00
struct tlv tag;
struct tlvdb *next;
struct tlvdb *parent;
struct tlvdb *children;
2017-11-11 03:08:28 +08:00
};
struct tlvdb_root {
2019-03-10 06:35:06 +08:00
struct tlvdb db;
size_t len;
unsigned char buf[0];
2017-11-11 03:08:28 +08:00
};
static tlv_tag_t tlv_parse_tag(const unsigned char **buf, size_t *len) {
2019-03-10 06:35:06 +08:00
tlv_tag_t tag;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (*len == 0)
return TLV_TAG_INVALID;
tag = **buf;
--*len;
++*buf;
if ((tag & TLV_TAG_VALUE_MASK) != TLV_TAG_VALUE_CONT)
return tag;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (*len == 0)
return TLV_TAG_INVALID;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
tag <<= 8;
tag |= **buf;
--*len;
++*buf;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return tag;
2017-11-11 03:08:28 +08:00
}
static size_t tlv_parse_len(const unsigned char **buf, size_t *len) {
2019-03-10 06:35:06 +08:00
size_t l;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (*len == 0)
return TLV_LEN_INVALID;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
l = **buf;
--*len;
++*buf;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (!(l & TLV_LEN_LONG))
return l;
2017-11-11 03:08:28 +08:00
2019-03-10 07:00:59 +08:00
size_t ll = l & ~ TLV_LEN_LONG;
2019-03-10 06:35:06 +08:00
if (ll > 5)
return TLV_LEN_INVALID;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
l = 0;
for (int i = 1; i <= ll; i++) {
l = (l << 8) + **buf;
--*len;
++*buf;
}
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return l;
2017-11-11 03:08:28 +08:00
}
bool tlv_parse_tl(const unsigned char **buf, size_t *len, struct tlv *tlv) {
2019-03-10 06:35:06 +08:00
tlv->value = 0;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
tlv->tag = tlv_parse_tag(buf, len);
if (tlv->tag == TLV_TAG_INVALID)
return false;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
tlv->len = tlv_parse_len(buf, len);
if (tlv->len == TLV_LEN_INVALID)
return false;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return true;
2017-11-11 03:08:28 +08:00
}
static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent);
static bool tlvdb_parse_one(struct tlvdb *tlvdb,
2019-03-10 07:00:59 +08:00
struct tlvdb *parent,
const unsigned char **tmp,
size_t *left) {
2019-03-10 06:35:06 +08:00
tlvdb->next = tlvdb->children = NULL;
tlvdb->parent = parent;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
tlvdb->tag.tag = tlv_parse_tag(tmp, left);
if (tlvdb->tag.tag == TLV_TAG_INVALID)
goto err;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
tlvdb->tag.len = tlv_parse_len(tmp, left);
if (tlvdb->tag.len == TLV_LEN_INVALID)
goto err;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (tlvdb->tag.len > *left)
goto err;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
tlvdb->tag.value = *tmp;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
*tmp += tlvdb->tag.len;
*left -= tlvdb->tag.len;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (tlv_is_constructed(&tlvdb->tag) && (tlvdb->tag.len != 0)) {
tlvdb->children = tlvdb_parse_children(tlvdb);
if (!tlvdb->children)
goto err;
} else {
tlvdb->children = NULL;
}
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return true;
2017-11-11 03:08:28 +08:00
err:
2019-03-10 06:35:06 +08:00
return false;
2017-11-11 03:08:28 +08:00
}
static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent) {
2019-03-10 06:35:06 +08:00
const unsigned char *tmp = parent->tag.value;
size_t left = parent->tag.len;
struct tlvdb *tlvdb, *first = NULL, *prev = NULL;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
while (left != 0) {
tlvdb = malloc(sizeof(*tlvdb));
if (prev)
prev->next = tlvdb;
else
first = tlvdb;
prev = tlvdb;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (!tlvdb_parse_one(tlvdb, parent, &tmp, &left))
goto err;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
tlvdb->parent = parent;
}
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return first;
2017-11-11 03:08:28 +08:00
err:
2019-03-10 06:35:06 +08:00
tlvdb_free(first);
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return NULL;
2017-11-11 03:08:28 +08:00
}
struct tlvdb *tlvdb_parse(const unsigned char *buf, size_t len) {
2019-03-10 06:35:06 +08:00
struct tlvdb_root *root;
const unsigned char *tmp;
size_t left;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (!len || !buf)
return NULL;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
root = malloc(sizeof(*root) + len);
root->len = len;
memcpy(root->buf, buf, len);
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
tmp = root->buf;
left = len;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
goto err;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (left)
goto err;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return &root->db;
2017-11-11 03:08:28 +08:00
err:
2019-03-10 06:35:06 +08:00
tlvdb_free(&root->db);
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return NULL;
2017-11-11 03:08:28 +08:00
}
struct tlvdb *tlvdb_parse_multi(const unsigned char *buf, size_t len) {
2019-03-10 06:35:06 +08:00
struct tlvdb_root *root;
const unsigned char *tmp;
size_t left;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (!len || !buf)
return NULL;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
root = malloc(sizeof(*root) + len);
root->len = len;
memcpy(root->buf, buf, len);
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
tmp = root->buf;
left = len;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
goto err;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
while (left != 0) {
struct tlvdb *db = malloc(sizeof(*db));
if (!tlvdb_parse_one(db, NULL, &tmp, &left)) {
2019-03-10 07:00:59 +08:00
free(db);
2019-03-10 06:35:06 +08:00
goto err;
}
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
tlvdb_add(&root->db, db);
}
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return &root->db;
2017-11-11 03:08:28 +08:00
err:
2019-03-10 06:35:06 +08:00
tlvdb_free(&root->db);
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return NULL;
2017-11-11 03:08:28 +08:00
}
struct tlvdb *tlvdb_fixed(tlv_tag_t tag, size_t len, const unsigned char *value) {
2019-03-10 06:35:06 +08:00
struct tlvdb_root *root = malloc(sizeof(*root) + len);
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
root->len = len;
memcpy(root->buf, value, len);
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
root->db.parent = root->db.next = root->db.children = NULL;
root->db.tag.tag = tag;
root->db.tag.len = len;
root->db.tag.value = root->buf;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return &root->db;
2017-11-11 03:08:28 +08:00
}
struct tlvdb *tlvdb_external(tlv_tag_t tag, size_t len, const unsigned char *value) {
2019-03-10 06:35:06 +08:00
struct tlvdb_root *root = malloc(sizeof(*root));
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
root->len = 0;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
root->db.parent = root->db.next = root->db.children = NULL;
root->db.tag.tag = tag;
root->db.tag.len = len;
root->db.tag.value = value;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return &root->db;
2017-11-11 03:08:28 +08:00
}
void tlvdb_free(struct tlvdb *tlvdb) {
2019-03-10 06:35:06 +08:00
struct tlvdb *next = NULL;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (!tlvdb)
return;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
for (; tlvdb; tlvdb = next) {
next = tlvdb->next;
tlvdb_free(tlvdb->children);
free(tlvdb);
}
2017-11-11 03:08:28 +08:00
}
struct tlvdb *tlvdb_find_next(struct tlvdb *tlvdb, tlv_tag_t tag) {
2019-03-10 06:35:06 +08:00
if (!tlvdb)
return NULL;
2019-03-10 06:35:06 +08:00
return tlvdb_find(tlvdb->next, tag);
2017-11-25 10:28:02 +08:00
}
struct tlvdb *tlvdb_find(struct tlvdb *tlvdb, tlv_tag_t tag) {
2019-03-10 06:35:06 +08:00
if (!tlvdb)
return NULL;
2019-03-10 06:35:06 +08:00
for (; tlvdb; tlvdb = tlvdb->next) {
if (tlvdb->tag.tag == tag)
return tlvdb;
}
2017-11-25 10:28:02 +08:00
2019-03-10 06:35:06 +08:00
return NULL;
2017-11-25 10:28:02 +08:00
}
struct tlvdb *tlvdb_find_full(struct tlvdb *tlvdb, tlv_tag_t tag) {
2019-03-10 06:35:06 +08:00
if (!tlvdb)
return NULL;
2019-03-10 06:35:06 +08:00
for (; tlvdb; tlvdb = tlvdb->next) {
if (tlvdb->tag.tag == tag)
return tlvdb;
2019-03-10 06:35:06 +08:00
if (tlvdb->children) {
2019-03-10 07:00:59 +08:00
struct tlvdb *ch = tlvdb_find_full(tlvdb->children, tag);
2019-03-10 06:35:06 +08:00
if (ch)
return ch;
}
}
2019-03-10 06:35:06 +08:00
return NULL;
}
struct tlvdb *tlvdb_find_path(struct tlvdb *tlvdb, tlv_tag_t tag[]) {
2019-03-10 06:35:06 +08:00
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;
2017-11-25 10:28:02 +08:00
}
void tlvdb_add(struct tlvdb *tlvdb, struct tlvdb *other) {
2019-03-10 06:35:06 +08:00
if (tlvdb == other)
return;
2019-03-10 06:35:06 +08:00
while (tlvdb->next) {
if (tlvdb->next == other)
return;
2019-03-10 06:35:06 +08:00
tlvdb = tlvdb->next;
}
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
tlvdb->next = other;
2017-11-11 03:08:28 +08:00
}
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) {
2019-03-10 06:35:06 +08:00
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);
2019-10-10 03:07:33 +08:00
bool tnewelm_linked = false;
2019-03-10 06:35:06 +08:00
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;
2019-10-10 03:07:33 +08:00
tnewelm_linked = true;
2019-03-10 06:35:06 +08:00
}
// 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;
2019-10-10 03:07:33 +08:00
tnewelm_linked = true;
2019-03-10 06:35:06 +08:00
break;
}
}
}
// free old element with childrens
telm->next = NULL;
tlvdb_free(telm);
2019-10-10 03:07:33 +08:00
if (tlvdb_elm) {
2019-03-10 06:35:06 +08:00
*tlvdb_elm = tnewelm;
2019-10-10 03:07:33 +08:00
tnewelm_linked = true;
}
if (! tnewelm_linked) {
tlvdb_free(tnewelm);
}
2019-03-10 06:35:06 +08:00
}
return;
}
void tlvdb_change_or_add_node(struct tlvdb *tlvdb, tlv_tag_t tag, size_t len, const unsigned char *value) {
2019-03-10 06:35:06 +08:00
tlvdb_change_or_add_node_ex(tlvdb, tag, len, value, NULL);
2019-01-10 22:22:32 +08:00
}
void tlvdb_visit(const struct tlvdb *tlvdb, tlv_cb cb, void *data, int level) {
2019-03-10 06:35:06 +08:00
struct tlvdb *next = NULL;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (!tlvdb)
return;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
for (; tlvdb; tlvdb = next) {
next = tlvdb->next;
cb(data, &tlvdb->tag, level, (tlvdb->children == NULL));
tlvdb_visit(tlvdb->children, cb, data, level + 1);
}
2017-11-11 03:08:28 +08:00
}
static const struct tlvdb *tlvdb_next(const struct tlvdb *tlvdb) {
2019-03-10 06:35:06 +08:00
if (tlvdb->children)
return tlvdb->children;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
while (tlvdb) {
if (tlvdb->next)
return tlvdb->next;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
tlvdb = tlvdb->parent;
}
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return NULL;
2017-11-11 03:08:28 +08:00
}
const struct tlv *tlvdb_get(const struct tlvdb *tlvdb, tlv_tag_t tag, const struct tlv *prev) {
2019-03-10 06:35:06 +08:00
if (prev) {
// tlvdb = tlvdb_next(container_of(prev, struct tlvdb, tag));
tlvdb = tlvdb_next((struct tlvdb *)prev);
}
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
while (tlvdb) {
if (tlvdb->tag.tag == tag)
return &tlvdb->tag;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
tlvdb = tlvdb_next(tlvdb);
}
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return NULL;
2017-11-11 03:08:28 +08:00
}
const struct tlv *tlvdb_get_inchild(const struct tlvdb *tlvdb, tlv_tag_t tag, const struct tlv *prev) {
2019-03-10 06:35:06 +08:00
tlvdb = tlvdb->children;
return tlvdb_get(tlvdb, tag, prev);
2017-11-25 10:28:02 +08:00
}
const struct tlv *tlvdb_get_tlv(const struct tlvdb *tlvdb) {
2019-03-10 06:35:06 +08:00
if (tlvdb)
return &tlvdb->tag;
else
return NULL;
}
unsigned char *tlv_encode(const struct tlv *tlv, size_t *len) {
2019-03-10 06:35:06 +08:00
size_t size = tlv->len;
unsigned char *data;
size_t pos;
if (tlv->tag > 0x100)
size += 2;
else
size += 1;
if (tlv->len > 0x7f)
size += 2;
else
size += 1;
data = malloc(size);
if (!data) {
*len = 0;
return NULL;
}
pos = 0;
if (tlv->tag > 0x100) {
data[pos++] = tlv->tag >> 8;
data[pos++] = tlv->tag & 0xff;
} else
data[pos++] = tlv->tag;
if (tlv->len > 0x7f) {
data[pos++] = 0x81;
data[pos++] = tlv->len;
} else
data[pos++] = tlv->len;
memcpy(data + pos, tlv->value, tlv->len);
pos += tlv->len;
*len = pos;
return data;
2017-11-11 03:08:28 +08:00
}
bool tlv_is_constructed(const struct tlv *tlv) {
2019-04-08 16:19:01 +08:00
return (((tlv->tag < 0x100 ? tlv->tag : tlv->tag >> 8) & TLV_TAG_COMPLEX) == TLV_TAG_COMPLEX);
2017-11-11 03:08:28 +08:00
}
bool tlv_equal(const struct tlv *a, const struct tlv *b) {
2019-03-10 06:35:06 +08:00
if (!a && !b)
return true;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
if (!a || !b)
return false;
2017-11-11 03:08:28 +08:00
2019-03-10 06:35:06 +08:00
return a->tag == b->tag && a->len == b->len && !memcmp(a->value, b->value, a->len);
2017-11-11 03:08:28 +08:00
}
struct tlvdb *tlvdb_elm_get_next(struct tlvdb *tlvdb) {
2019-03-10 06:35:06 +08:00
return tlvdb->next;
}
struct tlvdb *tlvdb_elm_get_children(struct tlvdb *tlvdb) {
2019-03-10 06:35:06 +08:00
return tlvdb->children;
}
struct tlvdb *tlvdb_elm_get_parent(struct tlvdb *tlvdb) {
2019-03-10 06:35:06 +08:00
return tlvdb->parent;
}
2019-01-04 01:42:40 +08:00
bool tlvdb_get_uint8(struct tlvdb *tlvRoot, tlv_tag_t tag, uint8_t *value) {
2019-03-10 06:35:06 +08:00
const struct tlv *tlvelm = tlvdb_get(tlvRoot, tag, NULL);
return tlv_get_uint8(tlvelm, value);
2019-01-11 00:33:21 +08:00
}
bool tlv_get_uint8(const struct tlv *etlv, uint8_t *value) {
2019-03-10 06:35:06 +08:00
*value = 0;
2019-03-10 07:00:59 +08:00
if (etlv) {
2019-03-10 06:35:06 +08:00
if (etlv->len == 0)
return true;
2019-03-10 07:00:59 +08:00
if (etlv->len == 1) {
2019-03-10 06:35:06 +08:00
*value = etlv->value[0];
return true;
}
}
return false;
2019-01-04 01:42:40 +08:00
}
bool tlv_get_int(const struct tlv *etlv, int *value) {
2019-03-10 06:35:06 +08:00
*value = 0;
2019-03-10 07:00:59 +08:00
if (etlv) {
2019-03-10 06:35:06 +08:00
if (etlv->len == 0)
return true;
2019-03-10 07:00:59 +08:00
if (etlv->len <= 4) {
for (int i = 0; i < etlv->len; i++) {
2019-03-10 06:35:06 +08:00
*value += etlv->value[i] * (1 << (i * 8));
}
return true;
}
}
return false;
2019-01-04 01:42:40 +08:00
}