git.zerfleddert.de Git - proxmark3-svn/blob - client/emv/tlv.c

   1 /*
   2  * libopenemv - a library to work with EMV family of smart cards
   3  * Copyright (C) 2012, 2015 Dmitry Eremin-Solenikov
   4  *
   5  * This library is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU Lesser General Public
   7  * License as published by the Free Software Foundation; either
   8  * version 2.1 of the License, or (at your option) any later version.
   9  *
  10  * This library is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13  * Lesser General Public License for more details.
  14  *
  15  * https://github.com/lumag/emv-tools/blob/master/lib/tlv.c
  16  */
  17
  18 #ifdef HAVE_CONFIG_H
  19 #include <config.h>
  20 #endif
  21
  22 #include "tlv.h"
  23
  24 #include <string.h>
  25 #include <stdint.h>
  26 #include <stddef.h>
  27 #include <stdlib.h>
  28
  29 #define TLV_TAG_CLASS_MASK      0xc0
  30 #define TLV_TAG_COMPLEX         0x20
  31 #define TLV_TAG_VALUE_MASK      0x1f
  32 #define TLV_TAG_VALUE_CONT      0x1f
  33 #define TLV_TAG_INVALID         0
  34
  35 #define TLV_LEN_LONG            0x80
  36 #define TLV_LEN_MASK            0x7f
  37 #define TLV_LEN_INVALID         (~0)
  38
  39 // http://radek.io/2012/11/10/magical-container_of-macro/
  40 //#define container_of(ptr, type, member) ({
  41 //      const typeof( ((type *)0)->member ) *__mptr = (ptr);
  42 //        (type *)( (char *)__mptr - offsetof(type,member) );})
  43
  44 struct tlvdb {
  45         struct tlv tag;
  46         struct tlvdb *next;
  47         struct tlvdb *parent;
  48         struct tlvdb *children;
  49 };
  50
  51 struct tlvdb_root {
  52         struct tlvdb db;
  53         size_t len;
  54         unsigned char buf[0];
  55 };
  56
  57 static tlv_tag_t tlv_parse_tag(const unsigned char **buf, size_t *len)
  58 {
  59         tlv_tag_t tag;
  60
  61         if (*len == 0)
  62                 return TLV_TAG_INVALID;
  63         tag = **buf;
  64         --*len;
  65         ++*buf;
  66         if ((tag & TLV_TAG_VALUE_MASK) != TLV_TAG_VALUE_CONT)
  67                 return tag;
  68
  69         if (*len == 0)
  70                 return TLV_TAG_INVALID;
  71
  72         tag <<= 8;
  73         tag |= **buf;
  74         --*len;
  75         ++*buf;
  76
  77         return tag;
  78 }
  79
  80 static size_t tlv_parse_len(const unsigned char **buf, size_t *len)
  81 {
  82         size_t l;
  83
  84         if (*len == 0)
  85                 return TLV_LEN_INVALID;
  86
  87         l = **buf;
  88         --*len;
  89         ++*buf;
  90
  91         if (!(l & TLV_LEN_LONG))
  92                 return l;
  93
  94         size_t ll = l &~ TLV_LEN_LONG;
  95         if (ll > 5)
  96                 return TLV_LEN_INVALID;
  97
  98         l = 0;
  99         for (int i = 1; i <= ll; i++) {
 100                 l = (l << 8) + **buf;
 101                 --*len;
 102                 ++*buf;
 103         }
 104
 105         return l;
 106 }
 107
 108 bool tlv_parse_tl(const unsigned char **buf, size_t *len, struct tlv *tlv)
 109 {
 110         tlv->value = 0;
 111
 112         tlv->tag = tlv_parse_tag(buf, len);
 113         if (tlv->tag == TLV_TAG_INVALID)
 114                 return false;
 115
 116         tlv->len = tlv_parse_len(buf, len);
 117         if (tlv->len == TLV_LEN_INVALID)
 118                 return false;
 119
 120         return true;
 121 }
 122
 123 static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent);
 124
 125 static bool tlvdb_parse_one(struct tlvdb *tlvdb,
 126                 struct tlvdb *parent,
 127                 const unsigned char **tmp,
 128                 size_t *left)
 129 {
 130         tlvdb->next = tlvdb->children = NULL;
 131         tlvdb->parent = parent;
 132
 133         tlvdb->tag.tag = tlv_parse_tag(tmp, left);
 134         if (tlvdb->tag.tag == TLV_TAG_INVALID)
 135                 goto err;
 136
 137         tlvdb->tag.len = tlv_parse_len(tmp, left);
 138         if (tlvdb->tag.len == TLV_LEN_INVALID)
 139                 goto err;
 140
 141         if (tlvdb->tag.len > *left)
 142                 goto err;
 143
 144         tlvdb->tag.value = *tmp;
 145
 146         *tmp += tlvdb->tag.len;
 147         *left -= tlvdb->tag.len;
 148
 149         if (tlv_is_constructed(&tlvdb->tag) && (tlvdb->tag.len != 0)) {
 150                 tlvdb->children = tlvdb_parse_children(tlvdb);
 151                 if (!tlvdb->children)
 152                         goto err;
 153         } else {
 154                 tlvdb->children = NULL;
 155         }
 156
 157         return true;
 158
 159 err:
 160         return false;
 161 }
 162
 163 static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent)
 164 {
 165         const unsigned char *tmp = parent->tag.value;
 166         size_t left = parent->tag.len;
 167         struct tlvdb *tlvdb, *first = NULL, *prev = NULL;
 168
 169         while (left != 0) {
 170                 tlvdb = malloc(sizeof(*tlvdb));
 171                 if (prev)
 172                         prev->next = tlvdb;
 173                 else
 174                         first = tlvdb;
 175                 prev = tlvdb;
 176
 177                 if (!tlvdb_parse_one(tlvdb, parent, &tmp, &left))
 178                         goto err;
 179
 180                 tlvdb->parent = parent;
 181         }
 182
 183         return first;
 184
 185 err:
 186         tlvdb_free(first);
 187
 188         return NULL;
 189 }
 190
 191 struct tlvdb *tlvdb_parse(const unsigned char *buf, size_t len)
 192 {
 193         struct tlvdb_root *root;
 194         const unsigned char *tmp;
 195         size_t left;
 196
 197         if (!len || !buf)
 198                 return NULL;
 199
 200         root = malloc(sizeof(*root) + len);
 201         root->len = len;
 202         memcpy(root->buf, buf, len);
 203
 204         tmp = root->buf;
 205         left = len;
 206
 207         if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
 208                 goto err;
 209
 210         if (left)
 211                 goto err;
 212
 213         return &root->db;
 214
 215 err:
 216         tlvdb_free(&root->db);
 217
 218         return NULL;
 219 }
 220
 221 struct tlvdb *tlvdb_parse_multi(const unsigned char *buf, size_t len)
 222 {
 223         struct tlvdb_root *root;
 224         const unsigned char *tmp;
 225         size_t left;
 226
 227         if (!len || !buf)
 228                 return NULL;
 229
 230         root = malloc(sizeof(*root) + len);
 231         root->len = len;
 232         memcpy(root->buf, buf, len);
 233
 234         tmp = root->buf;
 235         left = len;
 236
 237         if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
 238                 goto err;
 239
 240         while (left != 0) {
 241                 struct tlvdb *db = malloc(sizeof(*db));
 242                 if (!tlvdb_parse_one(db, NULL, &tmp, &left)) {
 243                         free (db);
 244                         goto err;
 245                 }
 246
 247                 tlvdb_add(&root->db, db);
 248         }
 249
 250         return &root->db;
 251
 252 err:
 253         tlvdb_free(&root->db);
 254
 255         return NULL;
 256 }
 257
 258 struct tlvdb *tlvdb_fixed(tlv_tag_t tag, size_t len, const unsigned char *value)
 259 {
 260         struct tlvdb_root *root = malloc(sizeof(*root) + len);
 261
 262         root->len = len;
 263         memcpy(root->buf, value, len);
 264
 265         root->db.parent = root->db.next = root->db.children = NULL;
 266         root->db.tag.tag = tag;
 267         root->db.tag.len = len;
 268         root->db.tag.value = root->buf;
 269
 270         return &root->db;
 271 }
 272
 273 struct tlvdb *tlvdb_external(tlv_tag_t tag, size_t len, const unsigned char *value)
 274 {
 275         struct tlvdb_root *root = malloc(sizeof(*root));
 276
 277         root->len = 0;
 278
 279         root->db.parent = root->db.next = root->db.children = NULL;
 280         root->db.tag.tag = tag;
 281         root->db.tag.len = len;
 282         root->db.tag.value = value;
 283
 284         return &root->db;
 285 }
 286
 287 void tlvdb_free(struct tlvdb *tlvdb)
 288 {
 289         struct tlvdb *next = NULL;
 290
 291         if (!tlvdb)
 292                 return;
 293
 294         for (; tlvdb; tlvdb = next) {
 295                 next = tlvdb->next;
 296                 tlvdb_free(tlvdb->children);
 297                 free(tlvdb);
 298         }
 299 }
 300
 301 struct tlvdb *tlvdb_find_next(struct tlvdb *tlvdb, tlv_tag_t tag) {
 302         if (!tlvdb)
 303                 return NULL;
 304
 305         return tlvdb_find(tlvdb->next, tag);
 306 }
 307
 308 struct tlvdb *tlvdb_find(struct tlvdb *tlvdb, tlv_tag_t tag) {
 309         if (!tlvdb)
 310                 return NULL;
 311
 312         for (; tlvdb; tlvdb = tlvdb->next) {
 313                 if (tlvdb->tag.tag == tag)
 314                         return tlvdb;
 315         }
 316
 317         return NULL;
 318 }
 319
 320 struct tlvdb *tlvdb_find_full(struct tlvdb *tlvdb, tlv_tag_t tag) {
 321         if (!tlvdb)
 322                 return NULL;
 323
 324         for (; tlvdb; tlvdb = tlvdb->next) {
 325                 if (tlvdb->tag.tag == tag)
 326                         return tlvdb;
 327
 328                 if (tlvdb->children) {
 329                         struct tlvdb * ch = tlvdb_find_full(tlvdb->children, tag);
 330                         if (ch)
 331                                 return ch;
 332                 }
 333         }
 334
 335         return NULL;
 336 }
 337
 338 struct tlvdb *tlvdb_find_path(struct tlvdb *tlvdb, tlv_tag_t tag[]) {
 339         int i = 0;
 340         struct tlvdb *tnext = tlvdb;
 341
 342         while (tnext && tag[i]) {
 343                 tnext = tlvdb_find(tnext, tag[i]);
 344                 i++;
 345                 if (tag[i] && tnext) {
 346                         tnext = tnext->children;
 347                 }
 348         }
 349
 350         return tnext;
 351 }
 352
 353 void tlvdb_add(struct tlvdb *tlvdb, struct tlvdb *other)
 354 {
 355         if (tlvdb == other)
 356                 return;
 357
 358         while (tlvdb->next) {
 359                 if (tlvdb->next == other)
 360                         return;
 361
 362                 tlvdb = tlvdb->next;
 363         }
 364
 365         tlvdb->next = other;
 366 }
 367
 368 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)
 369 {
 370         struct tlvdb *telm = tlvdb_find_full(tlvdb, tag);
 371         if (telm == NULL) {
 372                 // new tlv element
 373                 struct tlvdb *elm = tlvdb_fixed(tag, len, value);
 374                 tlvdb_add(tlvdb, elm);
 375                 if (tlvdb_elm)
 376                         *tlvdb_elm = elm;
 377         } else {
 378                 // the same tlv structure
 379                 if (telm->tag.tag == tag && telm->tag.len == len && !memcmp(telm->tag.value, value, len))
 380                         return;
 381
 382                 // replace tlv element
 383                 struct tlvdb *tnewelm = tlvdb_fixed(tag, len, value);
 384                 tnewelm->next = telm->next;
 385                 tnewelm->parent = telm->parent;
 386
 387                 // if telm stayed first in children chain
 388                 if (telm->parent && telm->parent->children == telm) {
 389                         telm->parent->children = tnewelm;
 390                 }
 391
 392                 // if telm have previous element
 393                 if (telm != tlvdb) {
 394                         // elm in root
 395                         struct tlvdb *celm = tlvdb;
 396                         // elm in child list of node
 397                         if (telm->parent && telm->parent->children)
 398                                 celm = telm->parent->children;
 399
 400                         // find previous element
 401                         for (; celm; celm = celm->next) {
 402                                 if (celm->next == telm) {
 403                                         celm->next = tnewelm;
 404                                         break;
 405                                 }
 406                         }
 407                 }
 408
 409                 // free old element with childrens
 410                 telm->next = NULL;
 411                 tlvdb_free(telm);
 412
 413                 if (tlvdb_elm)
 414                         *tlvdb_elm = tnewelm;
 415         }
 416
 417         return;
 418 }
 419
 420 void tlvdb_change_or_add_node(struct tlvdb *tlvdb, tlv_tag_t tag, size_t len, const unsigned char *value)
 421 {
 422         tlvdb_change_or_add_node_ex(tlvdb, tag, len, value, NULL);
 423 }
 424
 425 void tlvdb_visit(const struct tlvdb *tlvdb, tlv_cb cb, void *data, int level)
 426 {
 427         struct tlvdb *next = NULL;
 428
 429         if (!tlvdb)
 430                 return;
 431
 432         for (; tlvdb; tlvdb = next) {
 433                 next = tlvdb->next;
 434                 cb(data, &tlvdb->tag, level, (tlvdb->children == NULL));
 435                 tlvdb_visit(tlvdb->children, cb, data, level + 1);
 436         }
 437 }
 438
 439 static const struct tlvdb *tlvdb_next(const struct tlvdb *tlvdb)
 440 {
 441         if (tlvdb->children)
 442                 return tlvdb->children;
 443
 444         while (tlvdb) {
 445                 if (tlvdb->next)
 446                         return tlvdb->next;
 447
 448                 tlvdb = tlvdb->parent;
 449         }
 450
 451         return NULL;
 452 }
 453
 454 const struct tlv *tlvdb_get(const struct tlvdb *tlvdb, tlv_tag_t tag, const struct tlv *prev)
 455 {
 456         if (prev) {
 457 //              tlvdb = tlvdb_next(container_of(prev, struct tlvdb, tag));
 458                 tlvdb = tlvdb_next((struct tlvdb *)prev);
 459         }
 460
 461
 462         while (tlvdb) {
 463                 if (tlvdb->tag.tag == tag)
 464                         return &tlvdb->tag;
 465
 466                 tlvdb = tlvdb_next(tlvdb);
 467         }
 468
 469         return NULL;
 470 }
 471
 472 const struct tlv *tlvdb_get_inchild(const struct tlvdb *tlvdb, tlv_tag_t tag, const struct tlv *prev) {
 473         tlvdb = tlvdb->children;
 474         return tlvdb_get(tlvdb, tag, prev);
 475 }
 476
 477 const struct tlv *tlvdb_get_tlv(const struct tlvdb *tlvdb) {
 478         return &tlvdb->tag;
 479 }
 480
 481 unsigned char *tlv_encode(const struct tlv *tlv, size_t *len)
 482 {
 483         size_t size = tlv->len;
 484         unsigned char *data;
 485         size_t pos;
 486
 487         if (tlv->tag > 0x100)
 488                 size += 2;
 489         else
 490                 size += 1;
 491
 492         if (tlv->len > 0x7f)
 493                 size += 2;
 494         else
 495                 size += 1;
 496
 497         data = malloc(size);
 498         if (!data) {
 499                 *len = 0;
 500                 return NULL;
 501         }
 502
 503         pos = 0;
 504
 505         if (tlv->tag > 0x100) {
 506                 data[pos++] = tlv->tag >> 8;
 507                 data[pos++] = tlv->tag & 0xff;
 508         } else
 509                 data[pos++] = tlv->tag;
 510
 511         if (tlv->len > 0x7f) {
 512                 data[pos++] = 0x81;
 513                 data[pos++] = tlv->len;
 514         } else
 515                 data[pos++] = tlv->len;
 516
 517         memcpy(data + pos, tlv->value, tlv->len);
 518         pos += tlv->len;
 519
 520         *len = pos;
 521         return data;
 522 }
 523
 524 bool tlv_is_constructed(const struct tlv *tlv)
 525 {
 526         return (tlv->tag < 0x100 ? tlv->tag : tlv->tag >> 8) & TLV_TAG_COMPLEX;
 527 }
 528
 529 bool tlv_equal(const struct tlv *a, const struct tlv *b)
 530 {
 531         if (!a && !b)
 532                 return true;
 533
 534         if (!a || !b)
 535                 return false;
 536
 537         return a->tag == b->tag && a->len == b->len && !memcmp(a->value, b->value, a->len);
 538 }
 539
 540 struct tlvdb *tlvdb_elm_get_next(struct tlvdb *tlvdb)
 541 {
 542         return tlvdb->next;
 543 }
 544
 545 struct tlvdb *tlvdb_elm_get_children(struct tlvdb *tlvdb)
 546 {
 547         return tlvdb->children;
 548 }
 549
 550 struct tlvdb *tlvdb_elm_get_parent(struct tlvdb *tlvdb)
 551 {
 552         return tlvdb->parent;
 553 }
 554
 555 bool tlv_get_uint8(const struct tlv *etlv, uint8_t *value)
 556 {
 557         *value = 0;
 558         if (etlv)
 559         {
 560                 if (etlv->len == 0)
 561                         return true;
 562
 563                 if (etlv->len == 1)
 564                 {
 565                         *value = etlv->value[0];
 566                         return true;
 567                 }
 568         }
 569         return false;
 570 }
 571
 572 bool tlv_get_int(const struct tlv *etlv, int *value)
 573 {
 574         *value = 0;
 575         if (etlv)
 576         {
 577                 if (etlv->len == 0)
 578                         return true;
 579
 580                 if (etlv->len <= 4)
 581                 {
 582                         for (int i = 0; i < etlv->len; i++)
 583                         {
 584                                 *value += etlv->value[i] * (1 << (i * 8));
 585                         }
 586                         return true;
 587                 }
 588         }
 589         return false;
 590 }