mirror of
https://github.com/Next-Flip/Momentum-Firmware.git
synced 2026-04-25 03:29:58 -07:00
86 lines
2.4 KiB
C
86 lines
2.4 KiB
C
#include "iso7816.h"
|
|
|
|
// ISO7816-5
|
|
// Simple-TLV (§5.2.1)
|
|
// BER-TLV (§5.2.2)
|
|
TlvInfo iso7816_tlv_parse(const uint8_t* data) {
|
|
TlvInfo tlv;
|
|
|
|
// Simple-TLV: tag can be any value from 1 to 254 (not '00' or 'FF')
|
|
// BER-TLV: TODO describe
|
|
// 00000 - 11110 => 0 - 30 (single byte)
|
|
// 11111 00011111 - 11111 01111111 => 31 - 127 (2 byte)
|
|
// 11111 10000001 00000001 - 11111 11111111 01111111 => 128 - 16383 (3 byte)
|
|
|
|
tlv.tag = *(data++);
|
|
tlv.ber.constructed = ((tlv.tag & 0x20) != 0);
|
|
tlv.ber.class = (tlv.tag >> 6) & 0x03;
|
|
if((tlv.tag & 0x1f) == 0x1f) {
|
|
// BER-TLV, multi byte tag
|
|
tlv.tag <<= 8;
|
|
tlv.tag |= *(data++);
|
|
tlv.ber.tag = tlv.tag & 0x7f;
|
|
if(tlv.tag & 0x80) {
|
|
// BER-TLV, 3 byte tag
|
|
tlv.tag &= ~0x80;
|
|
tlv.tag <<= 7;
|
|
tlv.tag |= *(data++) & 0x7f;
|
|
tlv.ber.tag = tlv.tag & 0x3fff;
|
|
}
|
|
} else {
|
|
tlv.ber.tag = tlv.tag & 0x1f;
|
|
}
|
|
|
|
//TODO: check for invalid 'indefinite length'
|
|
tlv.length = *(data++);
|
|
if(tlv.length == 0xff) {
|
|
// Simple-TLV 2 byte length
|
|
tlv.length = *(data++) << 8;
|
|
tlv.length += *(data++);
|
|
} else if(tlv.length > 0x7f) {
|
|
uint8_t length_bytes = tlv.length & 0x7f;
|
|
//printf("BER length of %d bytes\n", length_bytes);
|
|
if(length_bytes < 1 || length_bytes > 4) {
|
|
//TODO: error: ISO7816 doesn't support more than 4 length bytes
|
|
return (TlvInfo){.tag = 0};
|
|
}
|
|
tlv.length = 0;
|
|
for(uint8_t i = 0; i < length_bytes; ++i) {
|
|
//printf("byte %d: %02x\n", i, *data);
|
|
tlv.length <<= 8;
|
|
tlv.length |= *(data++);
|
|
}
|
|
}
|
|
tlv.value = data;
|
|
tlv.next = data + tlv.length;
|
|
|
|
return tlv;
|
|
}
|
|
|
|
TlvInfo
|
|
iso7816_tlv_select(const uint8_t* data, size_t length, const uint16_t tags[], size_t num_tags) {
|
|
TlvInfo tlv;
|
|
size_t offset = 0;
|
|
|
|
if(num_tags == 0) {
|
|
return (TlvInfo){.tag = 0x0000};
|
|
}
|
|
|
|
while(offset < length) {
|
|
tlv = iso7816_tlv_parse(data + offset);
|
|
|
|
if(tlv.tag == tags[0]) {
|
|
if(num_tags == 1) {
|
|
return tlv;
|
|
} else {
|
|
return iso7816_tlv_select(tlv.value, tlv.length, tags + 1, num_tags - 1);
|
|
}
|
|
}
|
|
|
|
offset =
|
|
tlv.next - data; // TODO: use some length value of TlvInfo instead of this monstrosity
|
|
}
|
|
|
|
return (TlvInfo){.tag = 0x0000};
|
|
}
|