From 66a507ba535d2f134fd1bb06bf9c163fdb58232f Mon Sep 17 00:00:00 2001
From: Chris van Marle <qistoph@gmail.com>
Date: Tue, 11 Oct 2022 22:13:08 +0200
Subject: [PATCH] Add ISO7816 tester

---
 lib/nfc/helpers/iso7816.c | 53 +++++++++++++++++++++++++++++++++++++
 lib/nfc/helpers/iso7816.h | 18 +++++++++++++
 test_iso7816_helpers.c    | 55 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 126 insertions(+)
 create mode 100644 lib/nfc/helpers/iso7816.c
 create mode 100644 lib/nfc/helpers/iso7816.h
 create mode 100644 test_iso7816_helpers.c

diff --git a/lib/nfc/helpers/iso7816.c b/lib/nfc/helpers/iso7816.c
new file mode 100644
index 000000000..5187aabbb
--- /dev/null
+++ b/lib/nfc/helpers/iso7816.c
@@ -0,0 +1,53 @@
+#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;
+
+	const uint8_t* org = data;
+
+	// 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++);
+	if ((tlv.tag & 0x1f) == 0x1f) {
+		// BER-TLV, multi byte tag
+		tlv.tag = *(data++);
+		if(tlv.tag & 0x80) {
+			// BER-TLV, 3 byte tag
+			tlv.tag &= ~0x80;
+			tlv.tag <<= 7;
+			tlv.tag |= *(data++) & 0x7f;
+		}
+	}
+
+	//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;
+}
diff --git a/lib/nfc/helpers/iso7816.h b/lib/nfc/helpers/iso7816.h
new file mode 100644
index 000000000..21e9bac34
--- /dev/null
+++ b/lib/nfc/helpers/iso7816.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <string.h>
+
+typedef struct {
+	uint16_t tag;
+	size_t length;
+	const uint8_t* value;
+
+	const uint8_t* next;
+} TlvInfo;
+
+// ISO7816-5 §5.2
+// Simple-TLV and BER-TLV parsing
+TlvInfo iso7816_tlv_parse(const uint8_t* data);
diff --git a/test_iso7816_helpers.c b/test_iso7816_helpers.c
new file mode 100644
index 000000000..afaedde75
--- /dev/null
+++ b/test_iso7816_helpers.c
@@ -0,0 +1,55 @@
+#include <stdio.h>
+
+#include "lib/nfc/helpers/iso7816.h"
+
+#define COLOR_RED "\033[0;31m"
+#define COLOR_GREEN "\033[0;32m"
+#define COLOR_RESET "\033[0;0m"
+
+void print_hex(const uint8_t* data, size_t length) {
+    for(size_t i=0; i<length; ++i) {
+        printf("%02X", data[i]);
+    }
+}
+
+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,
+                exp_tag, tlv.tag);
+        return;
+    }
+
+	if(tlv.length != exp_length) {
+        printf(COLOR_RED "FAILED  - iso7816_tlv_parse Length for ");
+		print_hex(input, input_size);
+		printf(" is not %ld, but %ld\n" COLOR_RESET,
+                exp_length, tlv.length);
+        return;
+    }
+
+    printf(COLOR_GREEN "SUCCESS - iso7816_tlv_parse for ");
+	print_hex(input, input_size);
+	printf(" is tag:%d, length:%ld\n" COLOR_RESET, tlv.tag, tlv.length);
+}
+
+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("\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);
+	test_iso7816_tlv_parse("\x4F\x82\x00\x05\x48\x65\x6C\x6C\x6F", 9, 0x4f, 5);
+	test_iso7816_tlv_parse("\x4F\x83\x00\x00\x05\x48\x65\x6C\x6C\x6F", 10, 0x4f, 5);
+	test_iso7816_tlv_parse("\x4F\x84\x00\x00\x00\x05\x48\x65\x6C\x6C\x6F", 11, 0x4f, 5);
+	test_iso7816_tlv_parse("\x4F\x85\x00\x00\x00\x00\x05\x48\x65\x6C\x6C\x6F", 12, 0, 0);
+
+	return 0;
+}