MRTD add helper functions:

- Check digit
- KMRZ

lib/nfc/nfc_worker.c

@@ -292,6 +292,7 @@ static bool nfc_worker_read_nfca(NfcWorker* nfc_worker, FuriHalNfcTxRxContext* t
 
             furi_hal_nfc_sleep(); // Needed between checks
             FURI_LOG_D(TAG, "Try reading MRTD");
+            //TODO: support NFC-B?
             if(nfc_worker_read_mrtd(nfc_worker, tx_rx)) {
                 nfc_worker->dev_data->protocol = NfcDeviceProtocolMRTD;
                 break;

lib/nfc/protocols/mrtd.h

@@ -2,6 +2,8 @@
 
 #include <furi_hal_nfc.h>
 
+#include "mrtd_helpers.h"
+
 typedef struct {
     FuriHalNfcTxRxContext* tx_rx;
     uint16_t file_offset;
@@ -11,31 +13,6 @@ typedef struct {
     uint64_t ssc_long;
 } MrtdApplication;
 
-typedef struct {
-    uint8_t year;
-    uint8_t month;
-    uint8_t day;
-} MrtdDate;
-
-// NULL terminated document ID
-#define MRTD_DOCNR_MAX_LENGTH 21
-
-typedef enum {
-    MrtdAuthMethodBac,
-    MrtdAuthMethodPace,
-} MrtdAuthMethod;
-
-typedef struct {
-    MrtdAuthMethod method;
-
-    // BAC input fields
-    MrtdDate birth_date;
-    MrtdDate expiry_date;
-    char doc_number[MRTD_DOCNR_MAX_LENGTH];
-
-    //TODO: PACE
-} MrtdAuthData;
-
 typedef struct {
     MrtdAuthData auth;
 } MrtdData;

lib/nfc/protocols/mrtd_helpers.c

@@ -0,0 +1,70 @@
+#include "mrtd_helpers.h"
+
+uint8_t mrtd_bac_check_digit(const uint8_t* input, const size_t length) {
+    const size_t num_weights = 3;
+    uint8_t weights[] = {7, 3, 1};
+    uint8_t check_digit = 0;
+    uint8_t idx;
+
+    for(size_t i=0; i<length; ++i) {
+        uint8_t c = input[i];
+        if(c >= 'A' && c <= 'Z') {
+            idx = c - 'A' + 10;
+        } else if(c >= 'a' && c <= 'z') {
+            idx = c - 'a' + 10;
+        } else if(c >= '0' && c <= '9') {
+            idx = c - '0';
+        } else {
+            idx = 0;
+        }
+        check_digit = (check_digit + idx * weights[i%num_weights]) % 10;
+    }
+    return check_digit;
+}
+
+void mrtd_print_date(uint8_t* output, MrtdDate* date) {
+    output[0] = (date->year / 10) + '0';
+    output[1] = (date->year % 10) + '0';
+    output[2] = (date->month / 10) + '0';
+    output[3] = (date->month % 10) + '0';
+    output[4] = (date->day / 10) + '0';
+    output[5] = (date->day % 10) + '0';
+}
+
+// Safe size: MRTD_DOCNR_MAX_LENGTH + 1 (CD) + 6 (DOB) + 1 (CD) + 6 (DOE) + 1 (CD) + 1 (0x00)
+// Test with:
+// - DOCNR of size 9
+// - DOCNR of size <9
+// - DOCNR of size >9
+// - DOCNR of size MRTD_DOCNR_MAX_LENGTH
+bool mrtd_bac_get_kmrz(MrtdAuthData* auth, uint8_t* output, size_t output_size) {
+    size_t idx = 0;
+    size_t docnr_length = strlen(auth->doc_number);
+    size_t cd_idx = 0;
+    if(output_size < docnr_length + 16) {
+        return false;
+    }
+
+    cd_idx = idx;
+    memcpy(output+idx, auth->doc_number, docnr_length);
+    idx += docnr_length;
+    if(docnr_length < 9) {
+        memset(output+idx, '<', 9-docnr_length);
+        idx += 9-docnr_length;
+    }
+
+    output[idx++] = mrtd_bac_check_digit(output+cd_idx, docnr_length) + '0';
+
+    cd_idx = idx;
+    mrtd_print_date(output+idx, &auth->birth_date);
+    idx += 6;
+    output[idx++] = mrtd_bac_check_digit(output+cd_idx, 6) + '0';
+
+    cd_idx = idx;
+    mrtd_print_date(output+idx, &auth->expiry_date);
+    idx += 6;
+    output[idx++] = mrtd_bac_check_digit(output+cd_idx, 6) + '0';
+
+    output[idx++] = '\x00';
+    return true;
+}

lib/nfc/protocols/mrtd_helpers.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <string.h>
+
+typedef struct {
+    uint8_t year;
+    uint8_t month;
+    uint8_t day;
+} MrtdDate;
+
+// NULL terminated document ID
+#define MRTD_DOCNR_MAX_LENGTH 21
+
+typedef enum {
+    MrtdAuthMethodBac,
+    MrtdAuthMethodPace,
+} MrtdAuthMethod;
+
+typedef struct {
+    MrtdAuthMethod method;
+
+    // BAC input fields
+    MrtdDate birth_date;
+    MrtdDate expiry_date;
+    char doc_number[MRTD_DOCNR_MAX_LENGTH];
+
+    //TODO: PACE
+} MrtdAuthData;
+
+uint8_t mrtd_bac_check_digit(const uint8_t* input, const size_t length);
+
+void mrtd_print_date(uint8_t* output, MrtdDate* date);
+
+bool mrtd_bac_get_kmrz(MrtdAuthData* auth, uint8_t* output, size_t output_size);

test_mrtd_helpers.c

@@ -0,0 +1,59 @@
+#include <stdio.h>
+
+#include "lib/nfc/protocols/mrtd_helpers.h"
+
+#define COLOR_RED "\033[0;31m"
+#define COLOR_GREEN "\033[0;32m"
+#define COLOR_RESET "\033[0;0m"
+
+void test_mrtd_bac_check_digit(const uint8_t* input, uint8_t exp_output) {
+    uint8_t output = mrtd_bac_check_digit(input, strlen(input));
+    if(output != exp_output) {
+        printf(COLOR_RED "FAILED  - mrtd_bac_check_digit for %s is not %d, but %d\n" COLOR_RESET,
+                input, exp_output, output);
+        return;
+    }
+
+    printf(COLOR_GREEN "SUCCESS - mrtd_bac_check_digit for %s is %d\n" COLOR_RESET,
+            input, output);
+}
+
+void test_bac_get_kmrz(MrtdAuthData* auth, uint8_t* exp_output) {
+    bool result;
+    uint8_t buffer[1000];
+
+    result = mrtd_bac_get_kmrz(auth, buffer, 1000);
+    if(!result) {
+        printf(COLOR_RED "FAILED  - mrtd_bac_get_kmrz returned FALSE for" COLOR_RESET);
+        return;
+    }
+
+    if(strcmp(exp_output, buffer)) {
+        printf(COLOR_RED "FAILED  - mrtd_bac_get_kmrz expected:\n%s, result:\n%s\n" COLOR_RESET,
+            exp_output,
+            buffer);
+    }
+
+    printf(COLOR_GREEN "SUCCESS - mrtd_bac_get_kmrz is: %s\n" COLOR_RESET,
+        buffer);
+}
+
+int main(int argc, char** argv) {
+    test_mrtd_bac_check_digit("D23145890734", 9);
+    test_mrtd_bac_check_digit("340712", 7);
+    test_mrtd_bac_check_digit("950712", 2);
+
+    MrtdAuthData mad1 = {
+        .doc_number = "D23145890734",
+        .birth_date = {34, 7, 12},
+        .expiry_date = {95, 7, 12},
+    };
+    test_bac_get_kmrz(&mad1, "D23145890734934071279507122");
+    test_bac_get_kmrz(&(MrtdAuthData){
+        .doc_number = "L898902C",
+        .birth_date = {69, 8, 6},
+        .expiry_date = {94, 6, 23},
+    }, "L898902C<369080619406236");
+
+    return 0;
+}