diff --git a/src/ccid/ccid2040.c b/src/ccid/ccid2040.c index df2fc2a..1184a0a 100644 --- a/src/ccid/ccid2040.c +++ b/src/ccid/ccid2040.c @@ -553,16 +553,20 @@ int format_tlv_len(size_t len, uint8_t *out) { return 0; } -int walk_tlv(const uint8_t *cdata, size_t cdata_len, uint8_t **p, uint8_t *tag, size_t *tag_len, uint8_t **data) { +int walk_tlv(const uint8_t *cdata, size_t cdata_len, uint8_t **p, uint16_t *tag, size_t *tag_len, uint8_t **data) { if (!p) return 0; if (!*p) *p = (uint8_t *)cdata; if (*p-cdata >= cdata_len) return 0; - uint8_t tg = 0x0; + uint16_t tg = 0x0; size_t tgl = 0; tg = *(*p)++; + if ((tg & 0x1f) == 0x1f) { + tg <<= 8; + tg |= *(*p)++; + } tgl = *(*p)++; if (tgl == 0x82) { tgl = *(*p)++ << 8; @@ -581,6 +585,16 @@ int walk_tlv(const uint8_t *cdata, size_t cdata_len, uint8_t **p, uint8_t *tag, return 1; } +bool asn1_find_tag(const uint8_t *data, size_t data_len, uint16_t itag, size_t *tag_len, uint8_t **tag_data) { + uint16_t tag = 0x0; + uint8_t *p = NULL; + while (walk_tlv(data, data_len, &p, &tag, tag_len, tag_data)) { + if (itag == tag) + return true; + } + return false; +} + void init_rtc() { rtc_init(); diff --git a/src/ccid/ccid2040.h b/src/ccid/ccid2040.h index faf3fd2..6a5efd2 100644 --- a/src/ccid/ccid2040.h +++ b/src/ccid/ccid2040.h @@ -244,7 +244,8 @@ extern void led_set_blink(uint32_t mode); #define CCID_WRONG_PADDING -1011 #define CCID_VERIFICATION_FAILED -1012 -extern int walk_tlv(const uint8_t *cdata, size_t cdata_len, uint8_t **p, uint8_t *tag, size_t *tag_len, uint8_t **data); +extern int walk_tlv(const uint8_t *cdata, size_t cdata_len, uint8_t **p, uint16_t *tag, size_t *tag_len, uint8_t **data); extern int format_tlv_len(size_t len, uint8_t *out); +extern bool asn1_find_tag(const uint8_t *data, size_t data_len, uint16_t itag, size_t *tag_len, uint8_t **tag_data); #endif \ No newline at end of file diff --git a/src/ccid/eac.c b/src/ccid/eac.c index 8fcf76c..09a8ccf 100644 --- a/src/ccid/eac.c +++ b/src/ccid/eac.c @@ -91,7 +91,8 @@ int sm_unwrap() { uint8_t *body = NULL; size_t body_size = 0; bool is87 = false; - uint8_t tag = 0x0, *tag_data = NULL, *p = NULL; + uint16_t tag = 0x0; + uint8_t *tag_data = NULL, *p = NULL; size_t tag_len = 0; while (walk_tlv(apdu.data, apdu.nc, &p, &tag, &tag_len, &tag_data)) { if (tag == 0x87 || tag == 0x85) { @@ -180,7 +181,8 @@ int sm_wrap() { } int sm_get_le() { - uint8_t tag = 0x0, *tag_data = NULL, *p = NULL; + uint16_t tag = 0x0; + uint8_t *tag_data = NULL, *p = NULL; size_t tag_len = 0; while (walk_tlv(apdu.data, apdu.nc, &p, &tag, &tag_len, &tag_data)) { if (tag == 0x97) { @@ -231,7 +233,8 @@ int sm_verify() { bool some_added = false; const uint8_t *mac = NULL; size_t mac_len = 0; - uint8_t tag = 0x0, *tag_data = NULL, *p = NULL; + uint16_t tag = 0x0; + uint8_t *tag_data = NULL, *p = NULL; size_t tag_len = 0; while (walk_tlv(apdu.data, apdu.nc, &p, &tag, &tag_len, &tag_data)) { if (tag & 0x1) { diff --git a/src/fs/file.c b/src/fs/file.c index 5d25cbd..96c81a1 100644 --- a/src/fs/file.c +++ b/src/fs/file.c @@ -305,7 +305,8 @@ int meta_find(uint16_t fid, uint8_t **out) { file_t *ef = search_by_fid(EF_META, NULL, SPECIFY_EF); if (!ef) return CCID_ERR_FILE_NOT_FOUND; - uint8_t tag = 0x0, *tag_data = NULL, *p = NULL, *data = file_get_data(ef); + uint16_t tag = 0x0; + uint8_t *tag_data = NULL, *p = NULL, *data = file_get_data(ef); size_t tag_len = 0, data_len = file_get_size(ef); while (walk_tlv(data, data_len, &p, &tag, &tag_len, &tag_data)) { if (tag_len < 2) @@ -323,7 +324,8 @@ int meta_delete(uint16_t fid) { file_t *ef = search_by_fid(EF_META, NULL, SPECIFY_EF); if (!ef) return CCID_ERR_FILE_NOT_FOUND; - uint8_t tag = 0x0, *tag_data = NULL, *p = NULL, *data = file_get_data(ef); + uint16_t tag = 0x0; + uint8_t *tag_data = NULL, *p = NULL, *data = file_get_data(ef); size_t tag_len = 0, data_len = file_get_size(ef); uint8_t *fdata = NULL; while (walk_tlv(data, data_len, &p, &tag, &tag_len, &tag_data)) { @@ -358,7 +360,8 @@ int meta_add(uint16_t fid, const uint8_t *data, uint16_t len) { uint16_t ef_size = file_get_size(ef); uint8_t *fdata = (uint8_t *)calloc(1, ef_size); memcpy(fdata, file_get_data(ef), ef_size); - uint8_t tag = 0x0, *tag_data = NULL, *p = NULL; + uint16_t tag = 0x0; + uint8_t *tag_data = NULL, *p = NULL; size_t tag_len = 0; while (walk_tlv(fdata, ef_size, &p, &tag, &tag_len, &tag_data)) { if (tag_len < 2) @@ -399,7 +402,7 @@ int meta_add(uint16_t fid, const uint8_t *data, uint16_t len) { } fdata = (uint8_t *)realloc(fdata, ef_size+1+format_tlv_len(len+2,NULL)+2+len); uint8_t *f = fdata+ef_size; - *f++ = fid & 0xff; + *f++ = fid & 0x1f; f += format_tlv_len(len+2, f); *f++ = fid >> 8; *f++ = fid & 0xff;