fix hf 15 reader (merge error)
[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;
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
109bool 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
124static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent);
125
126static 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
160err:
161 return false;
162}
163
164static 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
186err:
187 tlvdb_free(first);
188
189 return NULL;
190}
191
192struct 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
216err:
217 tlvdb_free(&root->db);
218
219 return NULL;
220}
221
222struct 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
253err:
254 tlvdb_free(&root->db);
255
256 return NULL;
257}
258
259struct 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
274struct 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
288void 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
3c5fce2b
OM
302struct 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
309struct 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
556826b5
OM
321struct 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
3c5fce2b
OM
339struct 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
a2bb2735 354void 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
556826b5
OM
363void 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
43912d63 409void tlvdb_visit(const struct tlvdb *tlvdb, tlv_cb cb, void *data, int level)
a2bb2735 410{
411 struct tlvdb *next = NULL;
412
413 if (!tlvdb)
414 return;
415
416 for (; tlvdb; tlvdb = next) {
417 next = tlvdb->next;
3c5fce2b
OM
418 cb(data, &tlvdb->tag, level, (tlvdb->children == NULL));
419 tlvdb_visit(tlvdb->children, cb, data, level + 1);
a2bb2735 420 }
421}
422
423static 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
438const 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
3c5fce2b
OM
456const 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
95b697f0
OM
461const struct tlv *tlvdb_get_tlv(const struct tlvdb *tlvdb) {
462 return &tlvdb->tag;
463}
464
a2bb2735 465unsigned char *tlv_encode(const struct tlv *tlv, size_t *len)
466{
467 size_t size = tlv->len;
468 unsigned char *data;
469 size_t pos;
470
471 if (tlv->tag > 0x100)
472 size += 2;
473 else
474 size += 1;
475
476 if (tlv->len > 0x7f)
477 size += 2;
478 else
479 size += 1;
480
481 data = malloc(size);
482 if (!data) {
483 *len = 0;
484 return NULL;
485 }
486
487 pos = 0;
488
489 if (tlv->tag > 0x100) {
490 data[pos++] = tlv->tag >> 8;
491 data[pos++] = tlv->tag & 0xff;
492 } else
493 data[pos++] = tlv->tag;
494
495 if (tlv->len > 0x7f) {
496 data[pos++] = 0x81;
497 data[pos++] = tlv->len;
498 } else
499 data[pos++] = tlv->len;
500
501 memcpy(data + pos, tlv->value, tlv->len);
502 pos += tlv->len;
503
504 *len = pos;
505 return data;
506}
507
508bool tlv_is_constructed(const struct tlv *tlv)
509{
510 return (tlv->tag < 0x100 ? tlv->tag : tlv->tag >> 8) & TLV_TAG_COMPLEX;
511}
512
513bool tlv_equal(const struct tlv *a, const struct tlv *b)
514{
515 if (!a && !b)
516 return true;
517
518 if (!a || !b)
519 return false;
520
521 return a->tag == b->tag && a->len == b->len && !memcmp(a->value, b->value, a->len);
522}
95b697f0
OM
523
524struct tlvdb *tlvdb_elm_get_next(struct tlvdb *tlvdb)
525{
526 return tlvdb->next;
527}
528
529struct tlvdb *tlvdb_elm_get_children(struct tlvdb *tlvdb)
530{
531 return tlvdb->children;
532}
533
534struct tlvdb *tlvdb_elm_get_parent(struct tlvdb *tlvdb)
535{
536 return tlvdb->parent;
537}
Impressum, Datenschutz