From 69ff30e86a1c4b97b38b8cb303a1ddb821ff524a Mon Sep 17 00:00:00 2001
From: Chris van Marle <qistoph@gmail.com>
Date: Tue, 11 Oct 2022 22:13:32 +0200
Subject: [PATCH] MRTD TLV parsing (and select)

---
 lib/nfc/helpers/iso7816.c |  34 +++++++++++-
 lib/nfc/helpers/iso7816.h |  17 +++---
 test_iso7816_helpers.c    | 109 +++++++++++++++++++++++++++++++-------
 3 files changed, 130 insertions(+), 30 deletions(-)

diff --git a/lib/nfc/helpers/iso7816.c b/lib/nfc/helpers/iso7816.c
index 2cbff0809..1759a67f9 100644
--- a/lib/nfc/helpers/iso7816.c
+++ b/lib/nfc/helpers/iso7816.c
@@ -13,15 +13,22 @@ TlvInfo iso7816_tlv_parse(const uint8_t* data) {
 	// 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 = *(data++);
+		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'
@@ -49,3 +56,28 @@ TlvInfo iso7816_tlv_parse(const uint8_t* data) {
 
 	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};
+}
diff --git a/lib/nfc/helpers/iso7816.h b/lib/nfc/helpers/iso7816.h
index 883da0d33..10d28d11b 100644
--- a/lib/nfc/helpers/iso7816.h
+++ b/lib/nfc/helpers/iso7816.h
@@ -11,17 +11,12 @@
 #define BER_CLASS_PRIVATE 0x3
 
 typedef struct {
-    union {
+    uint16_t tag; // TODO: use define/typedef for this data format?
+    struct {
         uint16_t tag;
-        struct {
-            // LSB
-            uint8_t tag : 5;
-            uint8_t constructed : 1;
-            uint8_t class : 2;
-            // MSB
-        } ber;
-        //TODO: currently only works for 1-byte tags
-    };
+        uint8_t constructed : 1;
+        uint8_t class : 2;
+    } ber;
 	size_t length;
 	const uint8_t* value;
 
@@ -31,3 +26,5 @@ typedef struct {
 // ISO7816-5 §5.2
 // Simple-TLV and BER-TLV parsing
 TlvInfo iso7816_tlv_parse(const uint8_t* data);
+
+TlvInfo iso7816_tlv_select(const uint8_t* data, size_t length, const uint16_t tags[], size_t num_tags);
diff --git a/test_iso7816_helpers.c b/test_iso7816_helpers.c
index e617f4713..292fefbf4 100644
--- a/test_iso7816_helpers.c
+++ b/test_iso7816_helpers.c
@@ -6,19 +6,31 @@
 #define COLOR_GREEN "\033[0;32m"
 #define COLOR_RESET "\033[0;0m"
 
+//TODO: do something with ISO7816-4 Table 9 — Interindustry data objects for tag allocation authority
+//0x06 Object identifier (encoding specified in ISO/IEC 8825-1, see examples in annex A)
+//0x41 Country code (encoding specified in ISO 3166-1 [1] ) and optional national data
+//0x42 Issuer identification number (encoding and registration specified in ISO/IEC 7812-1 [3] ) and optional issuer data
+//0x4F Application identifier (AID, encoding specified in 8.2.1.2)
+
 void print_hex(const uint8_t* data, size_t length) {
     for(size_t i=0; i<length; ++i) {
         printf("%02X", data[i]);
     }
 }
 
+void print_tlv(char* fmt, TlvInfo tlv) {
+    printf("%s Tag: %x, Length: %ld, Value: ", fmt, tlv.tag, tlv.length);
+    print_hex(tlv.value, tlv.length);
+    printf("\n");
+}
+
 void test_iso7816_tlv_parse(const uint8_t* input, size_t input_size, uint16_t exp_tag, size_t exp_length) {
 	TlvInfo tlv = iso7816_tlv_parse(input);
 
 	if(tlv.tag != exp_tag) {
         printf(COLOR_RED "FAILED  - iso7816_tlv_parse Tag for ");
 		print_hex(input, input_size);
-		printf(" is not %d, but %d\n" COLOR_RESET,
+		printf(" is not %1$d (%1$x), but %2$d (%2$x)\n" COLOR_RESET,
                 exp_tag, tlv.tag);
         return;
     }
@@ -58,7 +70,7 @@ void test_iso7816_tlv_parse_ber(const uint8_t* input, size_t input_size, uint8_t
 	if(tlv.ber.tag != exp_tag) {
         printf(COLOR_RED "FAILED  - iso7816_tlv_parse ber.tag for ");
 		print_hex(input, input_size);
-		printf(" is not %d, but %d\n" COLOR_RESET,
+		printf(" is not %1$d (%1$x), but %2$d (%2$x)\n" COLOR_RESET,
                 exp_tag, tlv.ber.tag);
         return;
     }
@@ -76,31 +88,75 @@ void test_iso7816_tlv_parse_ber(const uint8_t* input, size_t input_size, uint8_t
 	printf(" is class:%d, constructed:%d, tag:%d, length:%ld\n" COLOR_RESET, tlv.ber.class, tlv.ber.constructed, tlv.ber.tag, tlv.length);
 }
 
-void describe_tlv(const uint8_t* data, size_t length, size_t indent) {
-    TlvInfo tlv = iso7816_tlv_parse(data);
+//TODO: memcmp values above?
 
-    char prefix[indent+1];
-    memset(prefix, ' ', indent);
-    prefix[indent] = '\x00';
+void test_iso7816_tlv_select(const uint8_t* input, size_t input_size, const uint16_t tags[], size_t num_tags, uint16_t exp_tag, uint8_t* exp_data, size_t exp_data_length) {
+    TlvInfo tlv = iso7816_tlv_select(input, input_size, tags, num_tags);
 
-    printf("%sTag: %d (BER - class: %d, constr: %d, tag: %d)\n", prefix, tlv.tag, tlv.ber.class, tlv.ber.constructed, tlv.ber.tag);
-    printf("%sLength: %ld\n", prefix, tlv.length);
-    printf("%sValue: ", prefix);
-    print_hex(tlv.value, tlv.length);
-    printf("\n");
+    if(tlv.tag != exp_tag) {
+        printf(COLOR_RED "FAILED  - iso7816_tlv_select tag for ");
+		print_hex(input, input_size);
+		printf(" is not %d, but %d\n" COLOR_RESET,
+                exp_tag, tlv.tag);
+        return;
+    }
 
-    if(tlv.ber.constructed) {
-        describe_tlv(tlv.value, tlv.length, indent+2);
+    if(tlv.length != exp_data_length) {
+        printf(COLOR_RED "FAILED  - iso7816_tlv_select length for ");
+		print_hex(input, input_size);
+		printf(" is not %ld, but %ld\n" COLOR_RESET,
+                exp_data_length, tlv.length);
+        return;
+    }
+
+    if(memcmp(tlv.value, exp_data, tlv.length)) {
+        printf(COLOR_RED "FAILED  - iso7816_tlv_select value for ");
+		print_hex(input, input_size);
+		printf(" is not\n");
+        print_hex(exp_data, exp_data_length);
+        printf(", but\n");
+        print_hex(tlv.value, tlv.length);
+        printf(COLOR_RESET "\n");
+        return;
+    }
+
+    printf(COLOR_GREEN "SUCCESS - iso7816_tlv_select for ");
+	print_hex(input, input_size);
+	printf(" is tag:%d, length:%ld\n" COLOR_RESET, tlv.tag, tlv.length);
+}
+
+void describe_tlv(const uint8_t* data, size_t length, size_t level) {
+    size_t offset = 0;
+    char prefix[level+1];
+    memset(prefix, ' ', level);
+    prefix[level] = '\x00';
+
+    printf("%sDescribe TLV (lvl: %ld), size: %ld\n", prefix, level, length);
+
+    while(offset < length) {
+        TlvInfo tlv = iso7816_tlv_parse(data + offset);
+
+        printf("%sTag: %x (%d) (BER - class: %d, constr: %d, tag: %d)\n", prefix, tlv.tag, tlv.tag, tlv.ber.class, tlv.ber.constructed, tlv.ber.tag);
+        printf("%sLength: %ld\n", prefix, tlv.length);
+        printf("%sValue: ", prefix);
+        print_hex(tlv.value, tlv.length);
+        printf("\n");
+
+        if(tlv.ber.constructed) {
+            describe_tlv(tlv.value, tlv.length, level+1);
+        }
+
+        offset = tlv.next - data;
     }
 }
 
 int main(int argc, char** argv) {
 	test_iso7816_tlv_parse("\x0F\x05\x48\x65\x6C\x6C\x6F", 7, 15, 5);
-	test_iso7816_tlv_parse("\x5F\x0F\x05\x48\x65\x6C\x6C\x6F", 8, 15, 5);
-	test_iso7816_tlv_parse("\x5F\x1F\x05\x48\x65\x6C\x6C\x6F", 8, 31, 5);
-	test_iso7816_tlv_parse("\x5F\x7F\x05\x48\x65\x6C\x6C\x6F", 8, 127, 5);
-	test_iso7816_tlv_parse("\x5F\x81\x00\x05\x48\x65\x6C\x6C\x6F", 9, 128, 5);
-	test_iso7816_tlv_parse("\x5F\xFF\x7F\x05\x48\x65\x6C\x6C\x6F", 9, 16383, 5);
+	test_iso7816_tlv_parse_ber("\x5F\x0F\x05\x48\x65\x6C\x6C\x6F", 8, BER_CLASS_APPLICATION, 0, 15, 5);
+	test_iso7816_tlv_parse_ber("\x5F\x1F\x05\x48\x65\x6C\x6C\x6F", 8, BER_CLASS_APPLICATION, 0, 31, 5);
+	test_iso7816_tlv_parse_ber("\x5F\x7F\x05\x48\x65\x6C\x6C\x6F", 8, BER_CLASS_APPLICATION, 0, 127, 5);
+	test_iso7816_tlv_parse_ber("\x5F\x81\x00\x05\x48\x65\x6C\x6C\x6F", 9, BER_CLASS_APPLICATION, 0, 128, 5);
+	test_iso7816_tlv_parse_ber("\x5F\xFF\x7F\x05\x48\x65\x6C\x6C\x6F", 9, BER_CLASS_APPLICATION, 0, 16383, 5);
 	test_iso7816_tlv_parse("\x0F\xff\x00\x05\x48\x65\x6C\x6C\x6F", 9, 15, 5);
 	test_iso7816_tlv_parse("\x04\xff\x01\x00\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65\x65", 260, 4, 256);
 	test_iso7816_tlv_parse("\x4F\x81\x05\x48\x65\x6C\x6C\x6F", 8, 0x4f, 5);
@@ -112,14 +168,29 @@ int main(int argc, char** argv) {
 
     test_iso7816_tlv_parse_ber("\x8A\x02Hi", 4, BER_CLASS_CONTEXT, 0, 10, 2);
     test_iso7816_tlv_parse_ber("\x6A\x04\x8A\x02Hi", 6, BER_CLASS_APPLICATION, 1, 10, 4);
+    test_iso7816_tlv_parse_ber("\xDF\x8A\x7F\x02Hi", 4, BER_CLASS_PRIVATE, 0, 0x57f, 2);
 
+    printf("=====\nEF.DIR\n");
     const uint8_t *ef_dir_data = "\x61\x09\x4F\x07\xA0\x00\x00\x02\x47\x10\x01\x61\x09\x4F\x07\xA0\x00\x00\x02\x47\x20\x01";
     size_t ef_dir_data_len = 22;
     describe_tlv(ef_dir_data, ef_dir_data_len, 0);
 
+    printf("=====\nEF.CardAccess\n");
     const uint8_t *ef_cardaccess_data = "\x31\x14\x30\x12\x06\x0A\x04\x00\x7F\x00\x07\x02\x02\x04\x02\x04\x02\x01\x02\x02\x01\x0E\x90\x00";
     size_t ef_cardaccess_data_len = 24;
     describe_tlv(ef_cardaccess_data, ef_cardaccess_data_len, 0);
 
+    printf("=====\nEF.Com\n");
+    const uint8_t *ef_com_data = "\x60\x16\x5F\x01\x04\x30\x31\x30\x37\x5F\x36\x06\x30\x34\x30\x30\x30\x30\x5C\x04\x61\x75\x6F\x6E";
+    size_t ef_com_data_len = 24;
+    describe_tlv(ef_com_data, ef_com_data_len, 0);
+
+    printf("====\n");
+
+    TlvInfo tlv = iso7816_tlv_select(ef_com_data, ef_com_data_len, (uint16_t[]){0x60, 0x5f36}, 2);
+    print_tlv("0x60, 0x5f36:", tlv);
+
+    test_iso7816_tlv_select(ef_com_data, ef_com_data_len, (uint16_t[]){0x60, 0x5f36}, 2, 0x5f36, "\x30\x34\x30\x30\x30\x30", 6);
+
 	return 0;
 }