git.zerfleddert.de Git - proxmark3-svn/blob

1 /*

2 * libopenemv - a library to work with EMV family of smart cards

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 */

18 #ifdef HAVE_CONFIG_H

19 #include <config.h>

20 #endif

22 #include "tlv.h"

24 #include <string.h>

25 #include <stdint.h>

26 #include <stddef.h>

27 #include <stdlib.h>

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

35 #define TLV_LEN_LONG 0x80

36 #define TLV_LEN_MASK 0x7f

37 #define TLV_LEN_INVALID (~0)

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) );})

44 struct tlvdb {

45 struct tlv tag;

46 struct tlvdb *next;

47 struct tlvdb *parent;

48 struct tlvdb *children;

49 };

51 struct tlvdb_root {

52 struct tlvdb db;

53 size_t len;

54 unsigned char buf[0];

55 };

 static tlv_tag_t tlv_parse_tag(const unsigned char **buf, size_t *len)

58 {

59 tlv_tag_t tag;

         if (*len == 0)

62 return TLV_TAG_INVALID;

63 tag = **buf;

64 --*len;

65 ++*buf;

         if ((tag & TLV_TAG_VALUE_MASK) != TLV_TAG_VALUE_CONT)

67 return tag;

         if (*len == 0)

70 return TLV_TAG_INVALID;

72 tag <<= 8;

73 tag |= **buf;

74 --*len;

75 ++*buf;

77 return tag;

78 }

 static size_t tlv_parse_len(const unsigned char **buf, size_t *len)

81 {

82 size_t l;

         if (*len == 0)

85 return TLV_LEN_INVALID;

87 l = **buf;

88 --*len;

89 ++*buf;

91 if (!(l & TLV_LEN_LONG))

92 return l;

94 size_t ll = l &~ TLV_LEN_LONG;

95 if (*len < ll)

96 return TLV_LEN_INVALID;

98 /* FIXME */

         if (ll != 1)

100 return TLV_LEN_INVALID;

101

102 l = **buf;

103 --*len;

104 ++*buf;

105

106 return l;

107 }

108

 bool tlv_parse_tl(const unsigned char **buf, size_t *len, struct tlv *tlv)

110 {

111 tlv->value = 0;

112

         tlv->tag = tlv_parse_tag(buf, len);

         if (tlv->tag == TLV_TAG_INVALID)

115 return false;

116

         tlv->len = tlv_parse_len(buf, len);

         if (tlv->len == TLV_LEN_INVALID)

119 return false;

120

121 return true;

122 }

123

 static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent);

125

 static bool tlvdb_parse_one(struct tlvdb *tlvdb,

127 struct tlvdb *parent,

128 const unsigned char **tmp,

129 size_t *left)

130 {

         tlvdb->next = tlvdb->children = NULL;

132 tlvdb->parent = parent;

133

         tlvdb->tag.tag = tlv_parse_tag(tmp, left);

         if (tlvdb->tag.tag == TLV_TAG_INVALID)

136 goto err;

137

         tlvdb->tag.len = tlv_parse_len(tmp, left);

         if (tlvdb->tag.len == TLV_LEN_INVALID)

140 goto err;

141

         if (tlvdb->tag.len > *left)

143 goto err;

144

145 tlvdb->tag.value = *tmp;

146

         *tmp += tlvdb->tag.len;

         *left -= tlvdb->tag.len;

149

         if (tlv_is_constructed(&tlvdb->tag) && (tlvdb->tag.len != 0)) {

                 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

 static struct tlvdb *tlvdb_parse_children(struct tlvdb *parent)

165 {

         const unsigned char *tmp = parent->tag.value;

         size_t left = parent->tag.len;

         struct tlvdb *tlvdb, *first = NULL, *prev = NULL;

169

         while (left != 0) {

                 tlvdb = malloc(sizeof(*tlvdb));

172 if (prev)

173 prev->next = tlvdb;

174 else

175 first = tlvdb;

176 prev = tlvdb;

177

                 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

 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

         root = malloc(sizeof(*root) + len);

202 root->len = len;

         memcpy(root->buf, buf, len);

204

205 tmp = root->buf;

206 left = len;

207

         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

 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

         root = malloc(sizeof(*root) + len);

232 root->len = len;

         memcpy(root->buf, buf, len);

234

235 tmp = root->buf;

236 left = len;

237

         if (!tlvdb_parse_one(&root->db, NULL, &tmp, &left))

239 goto err;

240

         while (left != 0) {

                 struct tlvdb *db = malloc(sizeof(*db));

                 if (!tlvdb_parse_one(db, NULL, &tmp, &left)) {

244 free (db);

245 goto err;

246 }

247

                 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

 struct tlvdb *tlvdb_fixed(tlv_tag_t tag, size_t len, const unsigned char *value)

260 {

         struct tlvdb_root *root = malloc(sizeof(*root) + len);

262

263 root->len = len;

         memcpy(root->buf, value, len);

265

         root->db.parent = root->db.next = root->db.children = NULL;

         root->db.tag.tag = tag;

         root->db.tag.len = len;

         root->db.tag.value = root->buf;

270

271 return &root->db;

272 }

273

 struct tlvdb *tlvdb_external(tlv_tag_t tag, size_t len, const unsigned char *value)

275 {

         struct tlvdb_root *root = malloc(sizeof(*root));

277

278 root->len = 0;

279

         root->db.parent = root->db.next = root->db.children = NULL;

         root->db.tag.tag = tag;

         root->db.tag.len = len;

         root->db.tag.value = value;

284

285 return &root->db;

286 }

287

 void tlvdb_free(struct tlvdb *tlvdb)

289 {

290 struct tlvdb *next = NULL;

291

292 if (!tlvdb)

293 return;

294

         for (; tlvdb; tlvdb = next) {

296 next = tlvdb->next;

297 tlvdb_free(tlvdb->children);

298 free(tlvdb);

299 }

300 }

301

 void tlvdb_add(struct tlvdb *tlvdb, struct tlvdb *other)

303 {

304 while (tlvdb->next) {

305 tlvdb = tlvdb->next;

306 }

307

308 tlvdb->next = other;

309 }

310

 void tlvdb_visit(const struct tlvdb *tlvdb, tlv_cb cb, void *data, int level)

312 {

313 struct tlvdb *next = NULL;

314

315 if (!tlvdb)

316 return;

317

         for (; tlvdb; tlvdb = next) {

319 next = tlvdb->next;

                 bool is_leaf = (tlvdb->children == NULL);

                 cb(data, &tlvdb->tag, level, is_leaf);

                 tlvdb_visit(tlvdb->children, cb, data, level+1);

323 }

324 }

325

 static const struct tlvdb *tlvdb_next(const struct tlvdb *tlvdb)

327 {

328 if (tlvdb->children)

329 return tlvdb->children;

330

331 while (tlvdb) {

332 if (tlvdb->next)

333 return tlvdb->next;

334

335 tlvdb = tlvdb->parent;

336 }

337

338 return NULL;

339 }

340

 const struct tlv *tlvdb_get(const struct tlvdb *tlvdb, tlv_tag_t tag, const struct tlv *prev)

342 {

343 if (prev) {

344 // tlvdb = tlvdb_next(container_of(prev, struct tlvdb, tag));

                 tlvdb = tlvdb_next((struct tlvdb *)prev);

346 }

347

348

349 while (tlvdb) {

                 if (tlvdb->tag.tag == tag)

351 return &tlvdb->tag;

352

353 tlvdb = tlvdb_next(tlvdb);

354 }

355

356 return NULL;

357 }

358

 unsigned char *tlv_encode(const struct tlv *tlv, size_t *len)

360 {

361 size_t size = tlv->len;

362 unsigned char *data;

363 size_t pos;

364

         if (tlv->tag > 0x100)

366 size += 2;

367 else

368 size += 1;

369

         if (tlv->len > 0x7f)

371 size += 2;

372 else

373 size += 1;

374

375 data = malloc(size);

376 if (!data) {

377 *len = 0;

378 return NULL;

379 }

380

381 pos = 0;

382

         if (tlv->tag > 0x100) {

                 data[pos++] = tlv->tag >> 8;

                 data[pos++] = tlv->tag & 0xff;

386 } else

387 data[pos++] = tlv->tag;

388

         if (tlv->len > 0x7f) {

390 data[pos++] = 0x81;

391 data[pos++] = tlv->len;

392 } else

393 data[pos++] = tlv->len;

394

         memcpy(data + pos, tlv->value, tlv->len);

396 pos += tlv->len;

397

398 *len = pos;

399 return data;

400 }

401

 bool tlv_is_constructed(const struct tlv *tlv)

403 {

         return (tlv->tag < 0x100 ? tlv->tag : tlv->tag >> 8) & TLV_TAG_COMPLEX;

405 }

406

 bool tlv_equal(const struct tlv *a, const struct tlv *b)

408 {

409 if (!a && !b)

410 return true;

411

412 if (!a || !b)

413 return false;

414

         return a->tag == b->tag && a->len == b->len && !memcmp(a->value, b->value, a->len);

416 }