MRTD TLV parsing (and select)

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};
+}

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);