mirror of
https://github.com/Proxmark/proxmark3.git
synced 2024-11-21 04:50:14 -08:00
0b6efd01ec
* share getATR from smartcard.h/c * remove duplicates in tlv.h and add get_uint_8 * check ATS/ATR length
597 lines
11 KiB
C
597 lines
11 KiB
C
/*
|
|
* 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.
|
|
*
|
|
* 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 <stdint.h>
|
|
#include <stddef.h>
|
|
#include <stdlib.h>
|
|
|
|
#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
|
|
|
|
#define TLV_LEN_LONG 0x80
|
|
#define TLV_LEN_MASK 0x7f
|
|
#define TLV_LEN_INVALID (~0)
|
|
|
|
// http://radek.io/2012/11/10/magical-container_of-macro/
|
|
//#define container_of(ptr, type, member) ({
|
|
// const typeof( ((type *)0)->member ) *__mptr = (ptr);
|
|
// (type *)( (char *)__mptr - offsetof(type,member) );})
|
|
|
|
struct tlvdb {
|
|
struct tlv tag;
|
|
struct tlvdb *next;
|
|
struct tlvdb *parent;
|
|
struct tlvdb *children;
|
|
};
|
|
|
|
struct tlvdb_root {
|
|
struct tlvdb db;
|
|
size_t len;
|
|
unsigned char buf[0];
|
|
};
|
|
|
|
static tlv_tag_t tlv_parse_tag(const unsigned char **buf, size_t *len)
|
|
{
|
|
tlv_tag_t tag;
|
|
|
|
if (*len == 0)
|
|
return TLV_TAG_INVALID;
|
|
tag = **buf;
|
|
--*len;
|
|
++*buf;
|
|
if ((tag & TLV_TAG_VALUE_MASK) != TLV_TAG_VALUE_CONT)
|
|
return tag;
|
|
|
|
if (*len == 0)
|
|
return TLV_TAG_INVALID;
|
|
|
|
tag <<= 8;
|
|
tag |= **buf;
|
|
--*len;
|
|
++*buf;
|
|
|
|
return tag;
|
|
}
|
|
|
|
static size_t tlv_parse_len(const unsigned char **buf, size_t *len)
|
|
{
|
|
size_t l;
|
|
|
|
if (*len == 0)
|
|
return TLV_LEN_INVALID;
|
|
|
|
l = **buf;
|
|
--*len;
|
|
++*buf;
|
|
|
|
if (!(l & TLV_LEN_LONG))
|
|
return l;
|
|
|
|
size_t ll = l &~ TLV_LEN_LONG;
|
|
if (ll > 5)
|
|
return TLV_LEN_INVALID;
|
|
|
|
l = 0;
|
|
for (int i = 1; i <= ll; i++) {
|
|
l = (l << 8) + **buf;
|
|
--*len;
|
|
++*buf;
|
|
}
|
|
|
|
return l;
|
|
}
|
|
|
|
bool tlv_parse_tl(const unsigned char **buf, size_t *len, struct tlv *tlv)
|
|
{
|
|
tlv->value = 0;
|
|
|
|
tlv->tag = tlv_parse_tag(buf, len);
|
|
if (tlv->tag == TLV_TAG_INVALID)
|
|
return false;
|
|
|
|
tlv->len = tlv_parse_len(buf, len);
|
|
if (tlv->len == TLV_LEN_INVALID)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent);
|
|
|
|
static bool tlvdb_parse_one(struct tlvdb *tlvdb,
|
|
struct tlvdb *parent,
|
|
const unsigned char **tmp,
|
|
size_t *left)
|
|
{
|
|
tlvdb->next = tlvdb->children = NULL;
|
|
tlvdb->parent = parent;
|
|
|
|
tlvdb->tag.tag = tlv_parse_tag(tmp, left);
|
|
if (tlvdb->tag.tag == TLV_TAG_INVALID)
|
|
goto err;
|
|
|
|
tlvdb->tag.len = tlv_parse_len(tmp, left);
|
|
if (tlvdb->tag.len == TLV_LEN_INVALID)
|
|
goto err;
|
|
|
|
if (tlvdb->tag.len > *left)
|
|
goto err;
|
|
|
|
tlvdb->tag.value = *tmp;
|
|
|
|
*tmp += tlvdb->tag.len;
|
|
*left -= tlvdb->tag.len;
|
|
|
|
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;
|
|
}
|
|
|
|
return true;
|
|
|
|
err:
|
|
return false;
|
|
}
|
|
|
|
static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent)
|
|
{
|
|
const unsigned char *tmp = parent->tag.value;
|
|
size_t left = parent->tag.len;
|
|
struct tlvdb *tlvdb, *first = NULL, *prev = NULL;
|
|
|
|
while (left != 0) {
|
|
tlvdb = malloc(sizeof(*tlvdb));
|
|
if (prev)
|
|
prev->next = tlvdb;
|
|
else
|
|
first = tlvdb;
|
|
prev = tlvdb;
|
|
|
|
if (!tlvdb_parse_one(tlvdb, parent, &tmp, &left))
|
|
goto err;
|
|
|
|
tlvdb->parent = parent;
|
|
}
|
|
|
|
return first;
|
|
|
|
err:
|
|
tlvdb_free(first);
|
|
|
|
return NULL;
|
|
}
|
|
|
|
struct tlvdb *tlvdb_parse(const unsigned char *buf, size_t len)
|
|
{
|
|
struct tlvdb_root *root;
|
|
const unsigned char *tmp;
|
|
size_t left;
|
|
|
|
if (!len || !buf)
|
|
return NULL;
|
|
|
|
root = malloc(sizeof(*root) + len);
|
|
root->len = len;
|
|
memcpy(root->buf, buf, len);
|
|
|
|
tmp = root->buf;
|
|
left = len;
|
|
|
|
if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
|
|
goto err;
|
|
|
|
if (left)
|
|
goto err;
|
|
|
|
return &root->db;
|
|
|
|
err:
|
|
tlvdb_free(&root->db);
|
|
|
|
return NULL;
|
|
}
|
|
|
|
struct tlvdb *tlvdb_parse_multi(const unsigned char *buf, size_t len)
|
|
{
|
|
struct tlvdb_root *root;
|
|
const unsigned char *tmp;
|
|
size_t left;
|
|
|
|
if (!len || !buf)
|
|
return NULL;
|
|
|
|
root = malloc(sizeof(*root) + len);
|
|
root->len = len;
|
|
memcpy(root->buf, buf, len);
|
|
|
|
tmp = root->buf;
|
|
left = len;
|
|
|
|
if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
|
|
goto err;
|
|
|
|
while (left != 0) {
|
|
struct tlvdb *db = malloc(sizeof(*db));
|
|
if (!tlvdb_parse_one(db, NULL, &tmp, &left)) {
|
|
free (db);
|
|
goto err;
|
|
}
|
|
|
|
tlvdb_add(&root->db, db);
|
|
}
|
|
|
|
return &root->db;
|
|
|
|
err:
|
|
tlvdb_free(&root->db);
|
|
|
|
return NULL;
|
|
}
|
|
|
|
struct tlvdb *tlvdb_fixed(tlv_tag_t tag, size_t len, const unsigned char *value)
|
|
{
|
|
struct tlvdb_root *root = malloc(sizeof(*root) + len);
|
|
|
|
root->len = len;
|
|
memcpy(root->buf, value, len);
|
|
|
|
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;
|
|
|
|
return &root->db;
|
|
}
|
|
|
|
struct tlvdb *tlvdb_external(tlv_tag_t tag, size_t len, const unsigned char *value)
|
|
{
|
|
struct tlvdb_root *root = malloc(sizeof(*root));
|
|
|
|
root->len = 0;
|
|
|
|
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;
|
|
|
|
return &root->db;
|
|
}
|
|
|
|
void tlvdb_free(struct tlvdb *tlvdb)
|
|
{
|
|
struct tlvdb *next = NULL;
|
|
|
|
if (!tlvdb)
|
|
return;
|
|
|
|
for (; tlvdb; tlvdb = next) {
|
|
next = tlvdb->next;
|
|
tlvdb_free(tlvdb->children);
|
|
free(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)
|
|
{
|
|
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;
|
|
|
|
if (!tlvdb)
|
|
return;
|
|
|
|
for (; tlvdb; tlvdb = next) {
|
|
next = tlvdb->next;
|
|
cb(data, &tlvdb->tag, level, (tlvdb->children == NULL));
|
|
tlvdb_visit(tlvdb->children, cb, data, level + 1);
|
|
}
|
|
}
|
|
|
|
static const struct tlvdb *tlvdb_next(const struct tlvdb *tlvdb)
|
|
{
|
|
if (tlvdb->children)
|
|
return tlvdb->children;
|
|
|
|
while (tlvdb) {
|
|
if (tlvdb->next)
|
|
return tlvdb->next;
|
|
|
|
tlvdb = tlvdb->parent;
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
const struct tlv *tlvdb_get(const struct tlvdb *tlvdb, tlv_tag_t tag, const struct tlv *prev)
|
|
{
|
|
if (prev) {
|
|
// tlvdb = tlvdb_next(container_of(prev, struct tlvdb, tag));
|
|
tlvdb = tlvdb_next((struct tlvdb *)prev);
|
|
}
|
|
|
|
|
|
while (tlvdb) {
|
|
if (tlvdb->tag.tag == tag)
|
|
return &tlvdb->tag;
|
|
|
|
tlvdb = tlvdb_next(tlvdb);
|
|
}
|
|
|
|
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 *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;
|
|
}
|
|
|
|
bool tlv_is_constructed(const struct tlv *tlv)
|
|
{
|
|
return (tlv->tag < 0x100 ? tlv->tag : tlv->tag >> 8) & TLV_TAG_COMPLEX;
|
|
}
|
|
|
|
bool tlv_equal(const struct tlv *a, const struct tlv *b)
|
|
{
|
|
if (!a && !b)
|
|
return true;
|
|
|
|
if (!a || !b)
|
|
return false;
|
|
|
|
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);
|
|
}
|