Fix for USB uart slowness since PR #720 (#787)
[proxmark3-svn] / client / emv / tlv.c
CommitLineData
a2bb2735 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
44struct tlvdb {
45 struct tlv tag;
46 struct tlvdb *next;
47 struct tlvdb *parent;
48 struct tlvdb *children;
49};
50
51struct tlvdb_root {
52 struct tlvdb db;
53 size_t len;
54 unsigned char buf[0];
55};
56
57static 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
80static 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;
14290308 95 if (ll > 5)
a2bb2735 96 return TLV_LEN_INVALID;
97
14290308
OM
98 l = 0;
99 for (int i = 1; i <= ll; i++) {
100 l = (l << 8) + **buf;
101 --*len;
102 ++*buf;
103 }
a2bb2735 104
105 return l;
106}
107
108bool 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
123static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent);
124
125static 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
159err:
160 return false;
161}
162
163static 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
185err:
186 tlvdb_free(first);
187
188 return NULL;
189}
190
191struct 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
215err:
216 tlvdb_free(&root->db);
217
218 return NULL;
219}
220
221struct 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
252err:
253 tlvdb_free(&root->db);
254
255 return NULL;
256}
257
258struct 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
273struct 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
287void 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
3c5fce2b
OM
301struct 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
308struct 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
556826b5
OM
320struct 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
3c5fce2b
OM
338struct 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
a2bb2735 353void tlvdb_add(struct tlvdb *tlvdb, struct tlvdb *other)
354{
355 while (tlvdb->next) {
356 tlvdb = tlvdb->next;
357 }
358
359 tlvdb->next = other;
360}
361
4cdd63b2 362void tlvdb_change_or_add_node_ex(struct tlvdb *tlvdb, tlv_tag_t tag, size_t len, const unsigned char *value, struct tlvdb **tlvdb_elm)
556826b5
OM
363{
364 struct tlvdb *telm = tlvdb_find_full(tlvdb, tag);
365 if (telm == NULL) {
366 // new tlv element
4cdd63b2 367 struct tlvdb *elm = tlvdb_fixed(tag, len, value);
368 tlvdb_add(tlvdb, elm);
369 if (tlvdb_elm)
370 *tlvdb_elm = elm;
556826b5
OM
371 } else {
372 // the same tlv structure
373 if (telm->tag.tag == tag && telm->tag.len == len && !memcmp(telm->tag.value, value, len))
374 return;
375
376 // replace tlv element
377 struct tlvdb *tnewelm = tlvdb_fixed(tag, len, value);
378 tnewelm->next = telm->next;
379 tnewelm->parent = telm->parent;
380
381 // if telm stayed first in children chain
382 if (telm->parent && telm->parent->children == telm) {
383 telm->parent->children = tnewelm;
384 }
385
386 // if telm have previous element
387 if (telm != tlvdb) {
388 // elm in root
389 struct tlvdb *celm = tlvdb;
390 // elm in child list of node
391 if (telm->parent && telm->parent->children)
392 celm = telm->parent->children;
393
394 // find previous element
395 for (; celm; celm = celm->next) {
396 if (celm->next == telm) {
397 celm->next = tnewelm;
398 break;
399 }
400 }
401 }
402
403 // free old element with childrens
404 telm->next = NULL;
405 tlvdb_free(telm);
4cdd63b2 406
407 if (tlvdb_elm)
408 *tlvdb_elm = tnewelm;
556826b5
OM
409 }
410
411 return;
412}
413
4cdd63b2 414void tlvdb_change_or_add_node(struct tlvdb *tlvdb, tlv_tag_t tag, size_t len, const unsigned char *value)
415{
416 tlvdb_change_or_add_node_ex(tlvdb, tag, len, value, NULL);
417}
418
43912d63 419void tlvdb_visit(const struct tlvdb *tlvdb, tlv_cb cb, void *data, int level)
a2bb2735 420{
421 struct tlvdb *next = NULL;
422
423 if (!tlvdb)
424 return;
425
426 for (; tlvdb; tlvdb = next) {
427 next = tlvdb->next;
3c5fce2b
OM
428 cb(data, &tlvdb->tag, level, (tlvdb->children == NULL));
429 tlvdb_visit(tlvdb->children, cb, data, level + 1);
a2bb2735 430 }
431}
432
433static const struct tlvdb *tlvdb_next(const struct tlvdb *tlvdb)
434{
435 if (tlvdb->children)
436 return tlvdb->children;
437
438 while (tlvdb) {
439 if (tlvdb->next)
440 return tlvdb->next;
441
442 tlvdb = tlvdb->parent;
443 }
444
445 return NULL;
446}
447
448const struct tlv *tlvdb_get(const struct tlvdb *tlvdb, tlv_tag_t tag, const struct tlv *prev)
449{
450 if (prev) {
451// tlvdb = tlvdb_next(container_of(prev, struct tlvdb, tag));
452 tlvdb = tlvdb_next((struct tlvdb *)prev);
453 }
454
455
456 while (tlvdb) {
457 if (tlvdb->tag.tag == tag)
458 return &tlvdb->tag;
459
460 tlvdb = tlvdb_next(tlvdb);
461 }
462
463 return NULL;
464}
465
3c5fce2b
OM
466const struct tlv *tlvdb_get_inchild(const struct tlvdb *tlvdb, tlv_tag_t tag, const struct tlv *prev) {
467 tlvdb = tlvdb->children;
468 return tlvdb_get(tlvdb, tag, prev);
469}
470
95b697f0
OM
471const struct tlv *tlvdb_get_tlv(const struct tlvdb *tlvdb) {
472 return &tlvdb->tag;
473}
474
a2bb2735 475unsigned char *tlv_encode(const struct tlv *tlv, size_t *len)
476{
477 size_t size = tlv->len;
478 unsigned char *data;
479 size_t pos;
480
481 if (tlv->tag > 0x100)
482 size += 2;
483 else
484 size += 1;
485
486 if (tlv->len > 0x7f)
487 size += 2;
488 else
489 size += 1;
490
491 data = malloc(size);
492 if (!data) {
493 *len = 0;
494 return NULL;
495 }
496
497 pos = 0;
498
499 if (tlv->tag > 0x100) {
500 data[pos++] = tlv->tag >> 8;
501 data[pos++] = tlv->tag & 0xff;
502 } else
503 data[pos++] = tlv->tag;
504
505 if (tlv->len > 0x7f) {
506 data[pos++] = 0x81;
507 data[pos++] = tlv->len;
508 } else
509 data[pos++] = tlv->len;
510
511 memcpy(data + pos, tlv->value, tlv->len);
512 pos += tlv->len;
513
514 *len = pos;
515 return data;
516}
517
518bool tlv_is_constructed(const struct tlv *tlv)
519{
520 return (tlv->tag < 0x100 ? tlv->tag : tlv->tag >> 8) & TLV_TAG_COMPLEX;
521}
522
523bool tlv_equal(const struct tlv *a, const struct tlv *b)
524{
525 if (!a && !b)
526 return true;
527
528 if (!a || !b)
529 return false;
530
531 return a->tag == b->tag && a->len == b->len && !memcmp(a->value, b->value, a->len);
532}
95b697f0
OM
533
534struct tlvdb *tlvdb_elm_get_next(struct tlvdb *tlvdb)
535{
536 return tlvdb->next;
537}
538
539struct tlvdb *tlvdb_elm_get_children(struct tlvdb *tlvdb)
540{
541 return tlvdb->children;
542}
543
544struct tlvdb *tlvdb_elm_get_parent(struct tlvdb *tlvdb)
545{
546 return tlvdb->parent;
547}
4cdd63b2 548
549bool tlv_get_uint8(const struct tlv *etlv, uint8_t *value)
550{
551 *value = 0;
552 if (etlv)
553 {
554 if (etlv->len == 0)
555 return true;
556
557 if (etlv->len == 1)
558 {
559 *value = etlv->value[0];
560 return true;
561 }
562 }
563 return false;
564}
565
566bool tlv_get_int(const struct tlv *etlv, int *value)
567{
568 *value = 0;
569 if (etlv)
570 {
571 if (etlv->len == 0)
572 return true;
573
574 if (etlv->len <= 4)
575 {
576 for (int i = 0; i < etlv->len; i++)
577 {
578 *value += etlv->value[i] * (1 << (i * 8));
579 }
580 return true;
581 }
582 }
583 return false;
584}
Impressum, Datenschutz