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