cvs.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 (*len < ll)
  96                 return TLV_LEN_INVALID;
  97
  98         /* FIXME */
  99         if (ll != 1)
 100                 return TLV_LEN_INVALID;
 101
 102         l = **buf;
 103         --*len;
 104         ++*buf;
 105
 106         return l;
 107 }
 108
 109 bool tlv_parse_tl(const unsigned char **buf, size_t *len, struct tlv *tlv)
 110 {
 111         tlv->value = 0;
 112
 113         tlv->tag = tlv_parse_tag(buf, len);
 114         if (tlv->tag == TLV_TAG_INVALID)
 115                 return false;
 116
 117         tlv->len = tlv_parse_len(buf, len);
 118         if (tlv->len == TLV_LEN_INVALID)
 119                 return false;
 120
 121         return true;
 122 }
 123
 124 static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent);
 125
 126 static bool tlvdb_parse_one(struct tlvdb *tlvdb,
 127                 struct tlvdb *parent,
 128                 const unsigned char **tmp,
 129                 size_t *left)
 130 {
 131         tlvdb->next = tlvdb->children = NULL;
 132         tlvdb->parent = parent;
 133
 134         tlvdb->tag.tag = tlv_parse_tag(tmp, left);
 135         if (tlvdb->tag.tag == TLV_TAG_INVALID)
 136                 goto err;
 137
 138         tlvdb->tag.len = tlv_parse_len(tmp, left);
 139         if (tlvdb->tag.len == TLV_LEN_INVALID)
 140                 goto err;
 141
 142         if (tlvdb->tag.len > *left)
 143                 goto err;
 144
 145         tlvdb->tag.value = *tmp;
 146
 147         *tmp += tlvdb->tag.len;
 148         *left -= tlvdb->tag.len;
 149
 150         if (tlv_is_constructed(&tlvdb->tag) && (tlvdb->tag.len != 0)) {
 151                 tlvdb->children = tlvdb_parse_children(tlvdb);
 152                 if (!tlvdb->children)
 153                         goto err;
 154         } else {
 155                 tlvdb->children = NULL;
 156         }
 157
 158         return true;
 159
 160 err:
 161         return false;
 162 }
 163
 164 static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent)
 165 {
 166         const unsigned char *tmp = parent->tag.value;
 167         size_t left = parent->tag.len;
 168         struct tlvdb *tlvdb, *first = NULL, *prev = NULL;
 169
 170         while (left != 0) {
 171                 tlvdb = malloc(sizeof(*tlvdb));
 172                 if (prev)
 173                         prev->next = tlvdb;
 174                 else
 175                         first = tlvdb;
 176                 prev = tlvdb;
 177
 178                 if (!tlvdb_parse_one(tlvdb, parent, &tmp, &left))
 179                         goto err;
 180
 181                 tlvdb->parent = parent;
 182         }
 183
 184         return first;
 185
 186 err:
 187         tlvdb_free(first);
 188
 189         return NULL;
 190 }
 191
 192 struct tlvdb *tlvdb_parse(const unsigned char *buf, size_t len)
 193 {
 194         struct tlvdb_root *root;
 195         const unsigned char *tmp;
 196         size_t left;
 197
 198         if (!len || !buf)
 199                 return NULL;
 200
 201         root = malloc(sizeof(*root) + len);
 202         root->len = len;
 203         memcpy(root->buf, buf, len);
 204
 205         tmp = root->buf;
 206         left = len;
 207
 208         if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
 209                 goto err;
 210
 211         if (left)
 212                 goto err;
 213
 214         return &root->db;
 215
 216 err:
 217         tlvdb_free(&root->db);
 218
 219         return NULL;
 220 }
 221
 222 struct tlvdb *tlvdb_parse_multi(const unsigned char *buf, size_t len)
 223 {
 224         struct tlvdb_root *root;
 225         const unsigned char *tmp;
 226         size_t left;
 227
 228         if (!len || !buf)
 229                 return NULL;
 230
 231         root = malloc(sizeof(*root) + len);
 232         root->len = len;
 233         memcpy(root->buf, buf, len);
 234
 235         tmp = root->buf;
 236         left = len;
 237
 238         if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))
 239                 goto err;
 240
 241         while (left != 0) {
 242                 struct tlvdb *db = malloc(sizeof(*db));
 243                 if (!tlvdb_parse_one(db, NULL, &tmp, &left)) {
 244                         free (db);
 245                         goto err;
 246                 }
 247
 248                 tlvdb_add(&root->db, db);
 249         }
 250
 251         return &root->db;
 252
 253 err:
 254         tlvdb_free(&root->db);
 255
 256         return NULL;
 257 }
 258
 259 struct tlvdb *tlvdb_fixed(tlv_tag_t tag, size_t len, const unsigned char *value)
 260 {
 261         struct tlvdb_root *root = malloc(sizeof(*root) + len);
 262
 263         root->len = len;
 264         memcpy(root->buf, value, len);
 265
 266         root->db.parent = root->db.next = root->db.children = NULL;
 267         root->db.tag.tag = tag;
 268         root->db.tag.len = len;
 269         root->db.tag.value = root->buf;
 270
 271         return &root->db;
 272 }
 273
 274 struct tlvdb *tlvdb_external(tlv_tag_t tag, size_t len, const unsigned char *value)
 275 {
 276         struct tlvdb_root *root = malloc(sizeof(*root));
 277
 278         root->len = 0;
 279
 280         root->db.parent = root->db.next = root->db.children = NULL;
 281         root->db.tag.tag = tag;
 282         root->db.tag.len = len;
 283         root->db.tag.value = value;
 284
 285         return &root->db;
 286 }
 287
 288 void tlvdb_free(struct tlvdb *tlvdb)
 289 {
 290         struct tlvdb *next = NULL;
 291
 292         if (!tlvdb)
 293                 return;
 294
 295         for (; tlvdb; tlvdb = next) {
 296                 next = tlvdb->next;
 297                 tlvdb_free(tlvdb->children);
 298                 free(tlvdb);
 299         }
 300 }
 301
 302 struct tlvdb *tlvdb_find_next(struct tlvdb *tlvdb, tlv_tag_t tag) {
 303         if (!tlvdb)
 304                 return NULL;
 305
 306         return tlvdb_find(tlvdb->next, tag);
 307 }
 308
 309 struct tlvdb *tlvdb_find(struct tlvdb *tlvdb, tlv_tag_t tag) {
 310         if (!tlvdb)
 311                 return NULL;
 312
 313         for (; tlvdb; tlvdb = tlvdb->next) {
 314                 if (tlvdb->tag.tag == tag)
 315                         return tlvdb;
 316         }
 317
 318         return NULL;
 319 }
 320
 321 struct tlvdb *tlvdb_find_full(struct tlvdb *tlvdb, tlv_tag_t tag) {
 322         if (!tlvdb)
 323                 return NULL;
 324
 325         for (; tlvdb; tlvdb = tlvdb->next) {
 326                 if (tlvdb->tag.tag == tag)
 327                         return tlvdb;
 328
 329                 if (tlvdb->children) {
 330                         struct tlvdb * ch = tlvdb_find_full(tlvdb->children, tag);
 331                         if (ch)
 332                                 return ch;
 333                 }
 334         }
 335
 336         return NULL;
 337 }
 338
 339 struct tlvdb *tlvdb_find_path(struct tlvdb *tlvdb, tlv_tag_t tag[]) {
 340         int i = 0;
 341         struct tlvdb *tnext = tlvdb;
 342
 343         while (tnext && tag[i]) {
 344                 tnext = tlvdb_find(tnext, tag[i]);
 345                 i++;
 346                 if (tag[i] && tnext) {
 347                         tnext = tnext->children;
 348                 }
 349         }
 350
 351         return tnext;
 352 }
 353
 354 void tlvdb_add(struct tlvdb *tlvdb, struct tlvdb *other)
 355 {
 356         while (tlvdb->next) {
 357                 tlvdb = tlvdb->next;
 358         }
 359
 360         tlvdb->next = other;
 361 }
 362
 363 void tlvdb_change_or_add_node(struct tlvdb *tlvdb, tlv_tag_t tag, size_t len, const unsigned char *value)
 364 {
 365         struct tlvdb *telm = tlvdb_find_full(tlvdb, tag);
 366         if (telm == NULL) {
 367                 // new tlv element
 368                 tlvdb_add(tlvdb, tlvdb_fixed(tag, len, value));
 369         } else {
 370                 // the same tlv structure
 371                 if (telm->tag.tag == tag && telm->tag.len == len && !memcmp(telm->tag.value, value, len))
 372                         return;
 373
 374                 // replace tlv element
 375                 struct tlvdb *tnewelm = tlvdb_fixed(tag, len, value);
 376                 tnewelm->next = telm->next;
 377                 tnewelm->parent = telm->parent;
 378
 379                 // if telm stayed first in children chain
 380                 if (telm->parent && telm->parent->children == telm) {
 381                         telm->parent->children = tnewelm;
 382                 }
 383
 384                 // if telm have previous element
 385                 if (telm != tlvdb) {
 386                         // elm in root
 387                         struct tlvdb *celm = tlvdb;
 388                         // elm in child list of node
 389                         if (telm->parent && telm->parent->children)
 390                                 celm = telm->parent->children;
 391
 392                         // find previous element
 393                         for (; celm; celm = celm->next) {
 394                                 if (celm->next == telm) {
 395                                         celm->next = tnewelm;
 396                                         break;
 397                                 }
 398                         }
 399                 }
 400
 401                 // free old element with childrens
 402                 telm->next = NULL;
 403                 tlvdb_free(telm);
 404         }
 405
 406         return;
 407 }
 408
 409 void tlvdb_visit(const struct tlvdb *tlvdb, tlv_cb cb, void *data, int level)
 410 {
 411         struct tlvdb *next = NULL;
 412
 413         if (!tlvdb)
 414                 return;
 415
 416         for (; tlvdb; tlvdb = next) {
 417                 next = tlvdb->next;
 418                 cb(data, &tlvdb->tag, level, (tlvdb->children == NULL));
 419                 tlvdb_visit(tlvdb->children, cb, data, level + 1);
 420         }
 421 }
 422
 423 static const struct tlvdb *tlvdb_next(const struct tlvdb *tlvdb)
 424 {
 425         if (tlvdb->children)
 426                 return tlvdb->children;
 427
 428         while (tlvdb) {
 429                 if (tlvdb->next)
 430                         return tlvdb->next;
 431
 432                 tlvdb = tlvdb->parent;
 433         }
 434
 435         return NULL;
 436 }
 437
 438 const struct tlv *tlvdb_get(const struct tlvdb *tlvdb, tlv_tag_t tag, const struct tlv *prev)
 439 {
 440         if (prev) {
 441 //              tlvdb = tlvdb_next(container_of(prev, struct tlvdb, tag));
 442                 tlvdb = tlvdb_next((struct tlvdb *)prev);
 443         }
 444
 445
 446         while (tlvdb) {
 447                 if (tlvdb->tag.tag == tag)
 448                         return &tlvdb->tag;
 449
 450                 tlvdb = tlvdb_next(tlvdb);
 451         }
 452
 453         return NULL;
 454 }
 455
 456 const struct tlv *tlvdb_get_inchild(const struct tlvdb *tlvdb, tlv_tag_t tag, const struct tlv *prev) {
 457         tlvdb = tlvdb->children;
 458         return tlvdb_get(tlvdb, tag, prev);
 459 }
 460
 461 unsigned char *tlv_encode(const struct tlv *tlv, size_t *len)
 462 {
 463         size_t size = tlv->len;
 464         unsigned char *data;
 465         size_t pos;
 466
 467         if (tlv->tag > 0x100)
 468                 size += 2;
 469         else
 470                 size += 1;
 471
 472         if (tlv->len > 0x7f)
 473                 size += 2;
 474         else
 475                 size += 1;
 476
 477         data = malloc(size);
 478         if (!data) {
 479                 *len = 0;
 480                 return NULL;
 481         }
 482
 483         pos = 0;
 484
 485         if (tlv->tag > 0x100) {
 486                 data[pos++] = tlv->tag >> 8;
 487                 data[pos++] = tlv->tag & 0xff;
 488         } else
 489                 data[pos++] = tlv->tag;
 490
 491         if (tlv->len > 0x7f) {
 492                 data[pos++] = 0x81;
 493                 data[pos++] = tlv->len;
 494         } else
 495                 data[pos++] = tlv->len;
 496
 497         memcpy(data + pos, tlv->value, tlv->len);
 498         pos += tlv->len;
 499
 500         *len = pos;
 501         return data;
 502 }
 503
 504 bool tlv_is_constructed(const struct tlv *tlv)
 505 {
 506         return (tlv->tag < 0x100 ? tlv->tag : tlv->tag >> 8) & TLV_TAG_COMPLEX;
 507 }
 508
 509 bool tlv_equal(const struct tlv *a, const struct tlv *b)
 510 {
 511         if (!a && !b)
 512                 return true;
 513
 514         if (!a || !b)
 515                 return false;
 516
 517         return a->tag == b->tag && a->len == b->len && !memcmp(a->value, b->value, a->len);
 518 }