Merge pull request #369 from qistoph/mrtd

Mrtd TEST PR

lib/nfc/helpers/iso7816.c

@@ -0,0 +1,83 @@
+#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};
+}

lib/nfc/helpers/iso7816.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <string.h>
+
+#define BER_CLASS_UNIVERSAL 0x0
+#define BER_CLASS_APPLICATION 0x1
+#define BER_CLASS_CONTEXT 0x2
+#define BER_CLASS_PRIVATE 0x3
+
+typedef struct {
+    uint16_t tag; // TODO: use define/typedef for this data format?
+    struct {
+        uint16_t tag;
+        uint8_t constructed : 1;
+        uint8_t class : 2;
+    } ber;
+	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);
+
+TlvInfo iso7816_tlv_select(const uint8_t* data, size_t length, const uint16_t tags[], size_t num_tags);