client/src/emv/tlv.c

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