| 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 | } |
| 591 | |
| 592 | bool tlvdb_get_uint8(struct tlvdb *tlvRoot, tlv_tag_t tag, uint8_t *value) |
| 593 | { |
| 594 | const struct tlv *tlvelm = tlvdb_get(tlvRoot, tag, NULL); |
| 595 | return tlv_get_uint8(tlvelm, value); |
| 596 | } |