Commit | Line | Data |
---|---|---|
3c5fce2b OM |
1 | /* |
2 | * libopenemv - a library to work with EMV family of smart cards | |
3 | * Copyright (C) 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 | ||
16 | #ifdef HAVE_CONFIG_H | |
17 | #include <config.h> | |
18 | #endif | |
19 | ||
20 | #include "emv/dol.h" | |
21 | #include "emv/tlv.h" | |
22 | ||
23 | #include <stdlib.h> | |
24 | #include <string.h> | |
25 | ||
26 | static size_t dol_calculate_len(const struct tlv *tlv, size_t data_len) | |
27 | { | |
28 | if (!tlv) | |
29 | return 0; | |
30 | ||
31 | const unsigned char *buf = tlv->value; | |
32 | size_t left = tlv->len; | |
33 | size_t count = 0; | |
34 | ||
35 | while (left) { | |
36 | struct tlv tlv; | |
37 | if (!tlv_parse_tl(&buf, &left, &tlv)) | |
38 | return 0; | |
39 | ||
40 | count += tlv.len; | |
41 | ||
42 | /* Last tag can be of variable length */ | |
43 | if (tlv.len == 0 && left == 0) | |
44 | count = data_len; | |
45 | } | |
46 | ||
47 | return count; | |
48 | } | |
49 | ||
50 | struct tlv *dol_process(const struct tlv *tlv, const struct tlvdb *tlvdb, tlv_tag_t tag) | |
51 | { | |
52 | size_t res_len; | |
53 | if (!tlv || !(res_len = dol_calculate_len(tlv, 0))) { | |
54 | struct tlv *res_tlv = malloc(sizeof(*res_tlv)); | |
55 | ||
56 | res_tlv->tag = tag; | |
57 | res_tlv->len = 0; | |
58 | res_tlv->value = NULL; | |
59 | ||
60 | return res_tlv; | |
61 | } | |
62 | ||
63 | struct tlv *res_tlv = malloc(sizeof(*res_tlv) + res_len); | |
64 | if (!res_tlv) | |
65 | return NULL; | |
66 | ||
67 | const unsigned char *buf = tlv->value; | |
68 | size_t left = tlv->len; | |
69 | unsigned char *res = (unsigned char *)(res_tlv + 1); | |
70 | size_t pos = 0; | |
71 | ||
72 | while (left) { | |
73 | struct tlv cur_tlv; | |
74 | if (!tlv_parse_tl(&buf, &left, &cur_tlv) || pos + cur_tlv.len > res_len) { | |
75 | free(res_tlv); | |
76 | ||
77 | return NULL; | |
78 | } | |
79 | ||
80 | const struct tlv *tag_tlv = tlvdb_get(tlvdb, cur_tlv.tag, NULL); | |
81 | if (!tag_tlv) { | |
82 | memset(res + pos, 0, cur_tlv.len); | |
83 | } else if (tag_tlv->len > cur_tlv.len) { | |
84 | memcpy(res + pos, tag_tlv->value, cur_tlv.len); | |
85 | } else { | |
86 | // FIXME: cn data should be padded with 0xFF !!! | |
87 | memcpy(res + pos, tag_tlv->value, tag_tlv->len); | |
88 | memset(res + pos + tag_tlv->len, 0, cur_tlv.len - tag_tlv->len); | |
89 | } | |
90 | pos += cur_tlv.len; | |
91 | } | |
92 | ||
93 | res_tlv->tag = tag; | |
94 | res_tlv->len = res_len; | |
95 | res_tlv->value = res; | |
96 | ||
97 | return res_tlv; | |
98 | } | |
99 | ||
100 | struct tlvdb *dol_parse(const struct tlv *tlv, const unsigned char *data, size_t data_len) | |
101 | { | |
102 | if (!tlv) | |
103 | return NULL; | |
104 | ||
105 | const unsigned char *buf = tlv->value; | |
106 | size_t left = tlv->len; | |
107 | size_t res_len = dol_calculate_len(tlv, data_len); | |
108 | size_t pos = 0; | |
109 | struct tlvdb *db = NULL; | |
110 | ||
111 | if (res_len != data_len) | |
112 | return NULL; | |
113 | ||
114 | while (left) { | |
115 | struct tlv cur_tlv; | |
116 | if (!tlv_parse_tl(&buf, &left, &cur_tlv) || pos + cur_tlv.len > res_len) { | |
117 | tlvdb_free(db); | |
118 | return NULL; | |
119 | } | |
120 | ||
121 | /* Last tag can be of variable length */ | |
122 | if (cur_tlv.len == 0 && left == 0) | |
123 | cur_tlv.len = res_len - pos; | |
124 | ||
125 | struct tlvdb *tag_db = tlvdb_fixed(cur_tlv.tag, cur_tlv.len, data + pos); | |
126 | if (!db) | |
127 | db = tag_db; | |
128 | else | |
129 | tlvdb_add(db, tag_db); | |
130 | ||
131 | pos += cur_tlv.len; | |
132 | } | |
133 | ||
134 | return db; | |
135 | } |