git.zerfleddert.de Git - proxmark3-svn/blob - client/emv/dol.c

   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 }