From 0401e7081c99c3745c91f8d89fd7c87ee81cbe15 Mon Sep 17 00:00:00 2001
From: dorababu <dorababu@subcom.tech>
Date: Fri, 2 Sep 2022 10:36:03 +0530
Subject: [PATCH] Init
- Using existing teensy key firmware
- to build for esp32
---
platformio.ini | 15 +
src/main.cpp | 1678 ++++++++++++++++++++++++++++++++++++++++++++++++
2 files changed, 1693 insertions(+)
create mode 100644 platformio.ini
create mode 100644 src/main.cpp
diff --git a/platformio.ini b/platformio.ini
new file mode 100644
index 0000000..bc8f138
--- /dev/null
+++ b/platformio.ini
@@ -0,0 +1,15 @@
+[env:esp32_u2f]
+
+platform = espressif32
+framework = arduino
+board = esp32dev
+;upload_protocol = teensy-cli
+build_flags =
+; -D USB_RAWHID
+
+lib_deps =
+ Wire
+ mbed-team2/Arduino
+ dfrobot/DFRobot_DHT20@^1.0.0
+ https://github.com/OperatorFoundation/Crypto.git
+
diff --git a/src/main.cpp b/src/main.cpp
new file mode 100644
index 0000000..f1796e8
--- /dev/null
+++ b/src/main.cpp
@@ -0,0 +1,1678 @@
+/*
+ * U2F key using TeensyLC board
+ * The reference for this project
+ *
+ * CREDIT:
+ * -------
+ * https://github.com/yohanes/teensy-u2f
+ *
+ * MODIFICATIONS:
+ * --------------
+ * We have added fingerprint scanner inaddition.
+ */
+
+// Include Arduino.h if you are building with platformIO
+#include "sha256.h"
+#include "uECC.h"
+#include <Arduino.h>
+#include <EEPROM.h>
+#include <string.h>
+// for AES encryption
+#include <AES.h>
+#include <Crypto.h>
+
+// Library for fingerprint scanner
+#include "DFRobot_ID809.h"
+
+// FingerPrint scanner with serial3 communication
+#define FPSerial Serial1
+#define SHA256_SIZE 32
+
+#define CID_BROADCAST 0xffffffff // Broadcast channel id
+#define CID_VENDOR 0x80ffffff // cid received from vendor
+#define TYPE_MASK 0x80 // Frame type mask
+#define TYPE_INIT 0x80 // Initial frame identifier
+#define TYPE_CONT 0x00 // Continuation frame identifier
+
+#define U2FHID_PING (TYPE_INIT | 0x01) // Echo data through local processor only
+#define U2FHID_MSG (TYPE_INIT | 0x03) // Send U2F message frame
+#define U2FHID_LOCK (TYPE_INIT | 0x04) // Send lock channel command
+#define U2FHID_INIT (TYPE_INIT | 0x06) // Channel initialization
+#define U2FHID_WINK (TYPE_INIT | 0x08) // Send device identification wink
+#define U2FHID_ERROR (TYPE_INIT | 0x3f) // Error response
+
+#define IS_CONTINUATION_PACKET(x) ((x) < 0x80)
+#define IS_NOT_CONTINUATION_PACKET(x) ((x) >= 0x80)
+
+#define SW_NO_ERROR 0x9000
+#define SW_CONDITIONS_NOT_SATISFIED 0x6985
+#define SW_WRONG_DATA 0x6A80
+#define SW_WRONG_LENGTH 0x6700
+#define SW_INS_NOT_SUPPORTED 0x6D00
+#define SW_CLA_NOT_SUPPORTED 0x6E00
+
+#define APPEND_SW(x, v1, v2) \
+ do { \
+ (*x++) = v1; \
+ (*x++) = v2; \
+ } while (0)
+#define APPEND_SW_NO_ERROR(x) \
+ do { \
+ (*x++) = 0x90; \
+ (*x++) = 0x00; \
+ } while (0)
+
+// Errors
+#define ERR_NONE 0
+#define ERR_INVALID_CMD 1
+#define ERR_INVALID_PAR 2
+#define ERR_INVALID_LEN 3
+#define ERR_INVALID_SEQ 4
+#define ERR_MSG_TIMEOUT 5
+#define ERR_CHANNEL_BUSY 6
+#define ERR_LOCK_REQUIRED 10
+#define ERR_INVALID_CID 11
+#define ERR_OTHER 127
+
+#define U2F_INS_REGISTER 0x01
+#define U2F_INS_AUTHENTICATE 0x02
+#define U2F_INS_VERSION 0x03
+
+#define U2F_INS_FPHASH 0x60
+#define U2F_INS_HASHMATCH 0x61
+#define U2F_INS_PING 0x62
+#define U2F_INS_COUNT 0x63
+#define U2F_INS_KEYCLOAK_EN 0x64
+#define U2F_INS_KEYCLOAK_DIS 0x65
+
+#define U2F_INS_CLR_FPS 0x50
+#define U2F_INS_REG_FPS 0x51
+#define U2F_INS_DEL_FPS 0x52
+
+#define STATE_CHANNEL_AVAILABLE 0
+#define STATE_CHANNEL_WAIT_PACKET 1
+#define STATE_CHANNEL_WAIT_CONT 2
+#define STATE_CHANNEL_TIMEOUT 3
+#define STATE_LARGE_PACKET 4
+
+#define MAX_TOTAL_PACKET 7609
+#define TIMEOUT_VALUE 1000
+
+#define MAX_INITIAL_PACKET 57
+#define MAX_CONTINUATION_PACKET 59
+#define SET_MSG_LEN(b, v) \
+ do { \
+ (b)[5] = ((v) >> 8) & 0xff; \
+ (b)[6] = (v)&0xff; \
+ } while (0)
+
+#define U2FHID_IF_VERSION 2 // Current interface implementation version
+#define COLLECT_NUMBER 3
+
+#define DEBUG
+
+byte expected_next_packet;
+int large_data_len;
+int large_data_offset;
+byte large_buffer[1024];
+byte large_resp_buffer[1024];
+byte recv_buffer[64];
+byte resp_buffer[64];
+byte handle[64];
+byte sha256_hash[32];
+
+byte fp_hash[32];
+
+// AES encryption parameters
+AES128 aes128;
+byte key[16] = { 0x73, 0x75, 0x62, 0x63, 0x6f, 0x6d, 0x6b, 0x65, 0x79, 0x23,
+ 0x73, 0x63, 0x31, 0x30, 0x30, 0x33 };
+String padding_character = "/";
+byte buffer[65];
+byte cipher_buffer[65];
+byte buffer_tmp[17];
+
+byte hash[SHA256_SIZE];
+uint8_t temp[1008];
+uint8_t ID = 0, ret = 0;
+
+int LAST_MATCHED_ID = -1;
+
+#define MAX_CHANNEL 4
+
+void authenticateKey();
+void encrypt_handle();
+void decrypt_handle();
+void saveHash();
+void readHash();
+void compute_hash();
+// EEPROM OFFSET to store hash
+const uint16_t EEPROM_FP_HASH = 1696;
+const uint16_t EEPROM_KEYCLOAK_EN = 1694;
+
+const uint8_t KEYCLOAK_EN = 0XAA;
+const uint8_t KEYCLOAK_DIS = 0X00;
+
+// User presence check for U2f key
+
+bool button_pressed = false;
+bool hash_matched = false;
+// Privatekey
+const char attestation_key[] = "\x07\x50\x1f\xa8\x85\x01\x8d\xa8\x86\xf7\x54"
+ "\x8e\x13\x3d\x69\x92\x7b\x1d\x6d\xb6\x8e\x04"
+ "\x58\x3e\x89\x91\xd8\x18\x8f\x9f\x4f\x3d";
+
+const char attestation_der[]
+ = "\x30\x82\x02\xa3\x30\x82\x02\x49\xa0\x03\x02\x01"
+ "\x02\x02\x14\x4c\xcd\x5a\xfb\x82\xe1\x1f\x1d\x33"
+ "\x03\x2a\x57\xfe\xf1\x7e\x23\xf6\xf3\x1b\x9a\x30"
+ "\x0a\x06\x08\x2a\x86\x48\xce\x3d\x04\x03\x02\x30"
+ "\x81\xa6\x31\x0b\x30\x09\x06\x03\x55\x04\x06\x13"
+ "\x02\x49\x4e\x31\x12\x30\x10\x06\x03\x55\x04\x08"
+ "\x0c\x09\x4b\x61\x72\x6e\x61\x74\x61\x6b\x61\x31"
+ "\x12\x30\x10\x06\x03\x55\x04\x07\x0c\x09\x42\x61"
+ "\x6e\x67\x61\x6c\x6f\x72\x65\x31\x0f\x30\x0d\x06"
+ "\x03\x55\x04\x0a\x0c\x06\x53\x75\x62\x63\x6f\x6d"
+ "\x31\x0c\x30\x0a\x06\x03\x55\x04\x0b\x0c\x03\x49"
+ "\x6f\x54\x31\x2b\x30\x29\x06\x03\x55\x04\x03\x0c"
+ "\x22\x44\x41\x34\x31\x30\x30\x31\x20\x34\x42\x30"
+ "\x39\x30\x30\x33\x32\x20\x30\x30\x30\x46\x39\x30"
+ "\x31\x33\x20\x36\x38\x32\x37\x34\x45\x34\x35\x31"
+ "\x23\x30\x21\x06\x09\x2a\x86\x48\x86\xf7\x0d\x01"
+ "\x09\x01\x16\x14\x6d\x72\x64\x6f\x72\x61\x62\x61"
+ "\x62\x75\x40\x67\x6d\x61\x69\x6c\x2e\x63\x6f\x6d"
+ "\x30\x1e\x17\x0d\x32\x32\x30\x36\x32\x31\x30\x39"
+ "\x35\x30\x30\x34\x5a\x17\x0d\x33\x32\x30\x36\x31"
+ "\x38\x30\x39\x35\x30\x30\x34\x5a\x30\x81\xa6\x31"
+ "\x0b\x30\x09\x06\x03\x55\x04\x06\x13\x02\x49\x4e"
+ "\x31\x12\x30\x10\x06\x03\x55\x04\x08\x0c\x09\x4b"
+ "\x61\x72\x6e\x61\x74\x61\x6b\x61\x31\x12\x30\x10"
+ "\x06\x03\x55\x04\x07\x0c\x09\x42\x61\x6e\x67\x61"
+ "\x6c\x6f\x72\x65\x31\x0f\x30\x0d\x06\x03\x55\x04"
+ "\x0a\x0c\x06\x53\x75\x62\x63\x6f\x6d\x31\x0c\x30"
+ "\x0a\x06\x03\x55\x04\x0b\x0c\x03\x49\x6f\x54\x31"
+ "\x2b\x30\x29\x06\x03\x55\x04\x03\x0c\x22\x44\x41"
+ "\x34\x31\x30\x30\x31\x20\x34\x42\x30\x39\x30\x30"
+ "\x33\x32\x20\x30\x30\x30\x46\x39\x30\x31\x33\x20"
+ "\x36\x38\x32\x37\x34\x45\x34\x35\x31\x23\x30\x21"
+ "\x06\x09\x2a\x86\x48\x86\xf7\x0d\x01\x09\x01\x16"
+ "\x14\x6d\x72\x64\x6f\x72\x61\x62\x61\x62\x75\x40"
+ "\x67\x6d\x61\x69\x6c\x2e\x63\x6f\x6d\x30\x59\x30"
+ "\x13\x06\x07\x2a\x86\x48\xce\x3d\x02\x01\x06\x08"
+ "\x2a\x86\x48\xce\x3d\x03\x01\x07\x03\x42\x00\x04"
+ "\xab\xa6\x42\xa4\x08\x3c\x41\x84\x9e\xb9\x13\x0a"
+ "\xa7\xac\xc7\xdc\x6f\x9f\x25\x20\x99\x87\x91\xa2"
+ "\x9f\xf2\xf4\x52\xdd\x5e\xc0\x6f\x18\x8d\x70\xa9"
+ "\xb5\x37\xd9\x8d\x51\x5b\x99\x7d\xf8\x8a\x89\xdc"
+ "\xee\xbf\x43\x8d\x86\x02\x76\x85\x4c\x47\x04\x2c"
+ "\x77\x05\x0f\x91\xa3\x53\x30\x51\x30\x1d\x06\x03"
+ "\x55\x1d\x0e\x04\x16\x04\x14\xeb\x04\x59\x64\xac"
+ "\x8f\xec\xe8\x31\x03\x9e\x16\x2f\x90\x29\x59\x56"
+ "\xea\x40\xcb\x30\x1f\x06\x03\x55\x1d\x23\x04\x18"
+ "\x30\x16\x80\x14\xeb\x04\x59\x64\xac\x8f\xec\xe8"
+ "\x31\x03\x9e\x16\x2f\x90\x29\x59\x56\xea\x40\xcb"
+ "\x30\x0f\x06\x03\x55\x1d\x13\x01\x01\xff\x04\x05"
+ "\x30\x03\x01\x01\xff\x30\x0a\x06\x08\x2a\x86\x48"
+ "\xce\x3d\x04\x03\x02\x03\x48\x00\x30\x45\x02\x20"
+ "\x3a\x7a\xab\x72\x58\xb9\x84\x9d\xe9\x1e\x8f\x9d"
+ "\x2f\x7b\xd6\xbf\xbb\x55\xb5\xcb\xb6\xd7\xdc\xf6"
+ "\xf5\x50\xf8\xe2\x02\xfc\xe0\x77\x02\x21\x00\xb3"
+ "\xc3\x5f\x72\x09\x4a\x52\x1c\xac\x94\x9b\xb8\xa8"
+ "\x43\xc9\x83\xd9\x89\x08\xa4\x04\x0e\xfd\x6b\x7c"
+ "\x0f\x92\x06\xd1\x3f\xa9\xa3";
+
+// key handle: (private key + app parameter) ^ this array
+const char handlekey[] = "-SUBCOMS-DORACOM-COMPUTE-SUBCOMS-";
+
+const struct uECC_Curve_t* curve = uECC_secp256r1(); // P-256
+uint8_t private_k[36]; // 32
+uint8_t public_k[68]; // 64
+
+struct ch_state {
+ int cid;
+ byte state;
+ int last_millis;
+};
+
+ch_state channel_states[MAX_CHANNEL];
+
+// Encrypt handle using AES-128 encryption
+void encrypt_handle()
+{
+ // Encrypt 16 by 16
+ for (uint8_t i = 0; i < sizeof(handle); i = i + 16) {
+ for (uint8_t j = 0; j < 16; j++) {
+ buffer_tmp[j] = handle[i + j];
+ }
+ aes128.encryptBlock(buffer_tmp, buffer_tmp);
+ for (uint8_t j = 0; j < 16; j++) {
+ cipher_buffer[i + j] = buffer_tmp[j];
+ }
+ }
+
+ // Print Encrypted
+ Serial.print("(Hex) Cipher : ");
+ for (uint8_t i = 0; i < sizeof(handle); i++) {
+ Serial.printf("%02x", cipher_buffer[i]);
+ handle[i] = cipher_buffer[i];
+ }
+}
+
+// Decrypt message using AES-128 encryption.
+
+void decrypt_handle()
+{
+
+ // Decrypt 16 by 16
+ for (uint8_t i = 0; i < sizeof(handle); i = i + 16) {
+ for (uint8_t j = 0; j < 16; j++) {
+ buffer_tmp[j] = handle[i + j];
+ }
+ aes128.decryptBlock(buffer_tmp, buffer_tmp);
+ for (uint8_t j = 0; j < 16; j++) {
+ buffer[i + j] = buffer_tmp[j];
+ }
+ }
+
+ // Print Decrypted
+ Serial.print("(Hex) Clear : ");
+ for (uint8_t i = 0; i < sizeof(handle); i++) {
+ Serial.printf("%02x", buffer[i]);
+ handle[i] = buffer[i];
+ }
+}
+
+extern "C" {
+
+int RNG(uint8_t* dest, unsigned size)
+{
+ // Use the least-significant bits from the ADC for an unconnected pin (or
+ // connected to a source of random noise). This can take a long time to
+ // generate random data if the result of analogRead(0) doesn't change very
+ // frequently.
+ while (size) {
+ uint8_t val = 0;
+ for (unsigned i = 0; i < 8; ++i) {
+ int init = analogRead(0);
+ int count = 0;
+ while (analogRead(0) == init) {
+ ++count;
+ }
+
+ if (count == 0) {
+ val = (val << 1) | (init & 0x01);
+ } else {
+ val = (val << 1) | (count & 0x01);
+ }
+ }
+ *dest = val;
+ ++dest;
+ --size;
+ }
+ // NOTE: it would be a good idea to hash the resulting random data using
+ // SHA-256 or similar.
+ return 1;
+}
+
+} // extern "C"
+
+typedef struct SHA256_HashContext {
+ uECC_HashContext uECC;
+ SHA256_CTX ctx;
+} SHA256_HashContext;
+
+void init_SHA256(uECC_HashContext* base)
+{
+ SHA256_HashContext* context = (SHA256_HashContext*)base;
+ sha256_init(&context->ctx);
+}
+void update_SHA256(
+ uECC_HashContext* base, const uint8_t* message, unsigned message_size)
+{
+ SHA256_HashContext* context = (SHA256_HashContext*)base;
+ sha256_update(&context->ctx, message, message_size);
+}
+void finish_SHA256(uECC_HashContext* base, uint8_t* hash_result)
+{
+ SHA256_HashContext* context = (SHA256_HashContext*)base;
+ sha256_final(&context->ctx, hash_result);
+}
+
+DFRobot_ID809 fingerprint;
+
+void readFingerPrint()
+{
+
+ uint8_t ret = 0;
+ uint8_t fp_temp[1008];
+ uint16_t memoryAddress = 0;
+ byte stored_hash[SHA256_SIZE];
+
+ /*Set fingerprint LED ring mode, color, and number of blinks
+ Can be set as follows:
+ Parameter 1:<LEDMode>
+ eBreathing eFastBlink eKeepsOn eNormalClose
+ eFadeIn eFadeOut eSlowBlink
+ Parameter 2:<LEDColor>
+ eLEDGreen eLEDRed eLEDYellow eLEDBlue
+ eLEDCyan eLEDMagenta eLEDWhite
+ Parameter 3:<number of blinks> 0 represents blinking all the time
+ This parameter will only be valid in mode eBreathing, eFastBlink,
+ eSlowBlink
+ */
+
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eBreathing,
+ /*LEDColor = */ fingerprint.eLEDBlue,
+ /*blinkCount = */ 0);
+ // Serial.println("Please press down your finger");
+ /*Capture fingerprint image, Disable the collection timeout function
+ If succeed return 0, otherwise return ERR_ID809
+ */
+ // authenticateKey();
+ if ((fingerprint.collectionFingerprint(/*timeout=*/0)) != ERR_ID809) {
+ // fingerprint.collectionFingerprint(0);
+ /*Set fingerprint LED ring to quick blink in yellow 3 times*/
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eFastBlink,
+ /*LEDColor = */ fingerprint.eLEDYellow,
+ /*blinkCount = */ 3);
+ // Serial.println("Capturing succeeds");
+ // authenticateKey();
+ // Serial.println("Please release your finger");
+ /*Wait for finger to release
+ Return 1 when finger is detected, otherwise return 0
+ */
+
+ while (fingerprint.detectFinger())
+ ;
+ // authenticateKey();
+ // fingerprint.detectFinger();
+ /*Compare the captured fingerprint with all the fingerprints in the
+ fingerprint library Return fingerprint ID(1-80) if succeed, return
+ 0 when failed
+ */
+ ret = fingerprint.search();
+ /*Compare the captured fingerprint with a fingerprint of specific ID
+ Return fingerprint ID(1-80) if succeed, return 0 when failed
+ */
+ // authenticateKey();
+ // ret = fingerprint.verify(/*Fingerprint ID = */1);
+ if (ret != 0) {
+ /*Set fingerprint LED ring to always ON in green */
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eKeepsOn,
+ /*LEDColor = */ fingerprint.eLEDGreen,
+ /*blinkCount = */ 0);
+ LAST_MATCHED_ID = ret;
+#ifdef DEBUG
+ Serial.print("Matching succeeds,ID=");
+ Serial.println(ret);
+#endif
+ // authenticateKey();
+ // delay(1000);
+ // button_pressed = 1;
+ if (ret > 10) {
+
+#ifdef DebUG
+ Serial.println("Outof EEPROM memory");
+#endif
+ return;
+ }
+ fingerprint.getTemplate(ret, fp_temp);
+
+ SHA256_CTX ctx;
+ sha256_init(&ctx);
+ sha256_update(&ctx, fp_temp, sizeof(fp_temp));
+ sha256_final(&ctx, fp_hash);
+#ifdef Debug
+ Serial.println("-----------------------");
+ for (uint8_t i = 0; i < SHA256_SIZE; i++) {
+
+ if (fp_hash[i] < 0x10) {
+
+ Serial.print('0');
+ }
+
+ Serial.print(fp_hash[i], HEX);
+ Serial.print(" ");
+ }
+ Serial.println("-----------------------");
+#endif
+
+ memoryAddress = EEPROM_FP_HASH + ((ret - 1) * 32);
+
+#ifdef DEBUG
+ Serial.println(ret);
+ Serial.println("---------------------------");
+#endif
+ for (uint8_t i = 0; i < SHA256_SIZE; i++) {
+
+ stored_hash[i] = EEPROM.read(memoryAddress);
+#ifdef DEBUG
+ Serial.print(stored_hash[i], HEX);
+ Serial.print(" ");
+ // Serial.println(memoryAddress);
+#endif
+ memoryAddress++;
+ }
+ Serial.println("--------------------------");
+
+ for (uint8_t i = 0; i < SHA256_SIZE; i++) {
+
+ if (stored_hash[i] != fp_hash[i]) {
+#ifdef DEBUG
+ Serial.println("Hash not matched");
+#endif
+ button_pressed = 0;
+
+ return;
+ }
+ }
+ button_pressed = true;
+#ifdef DEBUG
+ Serial.println("Hash matched");
+#endif
+
+ } else {
+ /*Set fingerprint LED ring to always ON in red*/
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eKeepsOn,
+ /*LEDColor = */ fingerprint.eLEDRed,
+ /*blinkCount = */ 0);
+ // Serial.println("Matching fails");
+ // delay(1000);
+ // authenticateKey();
+ button_pressed = false;
+ }
+ } else {
+ // Serial.println("Capturing fails");
+ /*Get error code information*/
+ // desc = fingerprint.getErrorDescription();
+ // Serial.println(desc);
+ // delay(1000);
+ // authenticateKey();
+ button_pressed = false;
+ }
+ // //Serial.println("-----------------------------");
+ // delay(1000);
+ // authenticateKey();
+}
+
+/*---------Compute hash----------------*/
+
+void compute_hash()
+{
+
+ SHA256_CTX ctx;
+ sha256_init(&ctx);
+ sha256_update(&ctx, temp, sizeof(temp));
+ sha256_final(&ctx, hash);
+
+ for (uint8_t i = 0; i < SHA256_SIZE; i++) {
+
+ if (hash[i] < 0x10) {
+
+ Serial.print('0');
+ }
+
+ Serial.print(hash[i], HEX);
+ Serial.print(" ");
+ }
+ Serial.println("\n");
+ Serial.println("-----------------");
+}
+
+/*-------- Save FingerPrint hash in EEPROM ------*/
+
+void saveHash()
+{
+ uint16_t memoryAddress;
+
+ memoryAddress = EEPROM_FP_HASH + ((ID - 1) * 32);
+
+ if (memoryAddress > 2016) {
+ Serial.println("Error: not enough space in"
+ " EEPROM");
+ return;
+ }
+
+ for (uint8_t i = 0; i < SHA256_SIZE; i++) {
+
+ EEPROM.write(memoryAddress, hash[i]);
+ memoryAddress++;
+ }
+}
+
+char U_ID[32];
+uint8_t HASH_DATA[32] = {};
+
+/*-------Read stored hash in EEPROM*/
+void readHash()
+{
+
+ uint16_t memoryAddress;
+ byte eeprom_data;
+
+ memoryAddress = EEPROM_FP_HASH + ((ID - 1) * 32);
+
+ uint8_t idx = 0;
+ for (uint16_t i = memoryAddress; i < (memoryAddress + SHA256_SIZE); i++) {
+
+ eeprom_data = EEPROM.read(i);
+ HASH_DATA[idx++] = eeprom_data;
+ Serial.print(eeprom_data, HEX);
+ Serial.print(" ");
+ }
+ Serial.println("\n");
+ Serial.println("--------------------");
+}
+
+/*-----------Arduino setup function-----------*/
+void setup()
+{
+ uECC_set_rng(&RNG);
+ FPSerial.begin(115200);
+ fingerprint.begin(FPSerial);
+ Serial.begin(9600);
+#ifdef DEBUG
+#endif
+ while (fingerprint.isConnected() == false) {
+ // Serial.println("Communication with device failed, please check
+ // connection");
+ /*Get error code information*/
+ // desc = fingerprint.getErrorDescription();
+ // Serial.println(desc);
+ delay(1000);
+ }
+#ifdef DEBUG
+ Serial.println(fingerprint.getEnrollCount());
+#endif
+
+ sprintf(U_ID, "%08lX %08lX %08lX %08lX", SIM_UIDH, SIM_UIDMH, SIM_UIDML,
+ SIM_UIDL);
+}
+
+void cleanup_timeout()
+{
+ int i;
+ for (i = 0; i < MAX_CHANNEL; i++) {
+ // free channel that is inactive
+ ch_state& c = channel_states[i];
+ int m = millis();
+ if (c.state != STATE_CHANNEL_AVAILABLE) {
+ if ((m - c.last_millis) > TIMEOUT_VALUE) {
+ c.state = STATE_CHANNEL_AVAILABLE;
+ }
+ }
+ }
+}
+
+int allocate_new_channel()
+{
+ int i;
+ // alloace new channel_id
+ int channel_id = 1;
+
+ do {
+ bool found = false;
+ for (i = 0; i < MAX_CHANNEL; i++) {
+ if (channel_states[i].state != STATE_CHANNEL_AVAILABLE) {
+ if (channel_states[i].cid == channel_id) {
+ found = true;
+ channel_id++;
+ break;
+ }
+ }
+ }
+ if (!found)
+ break;
+ } while (true);
+ return channel_id;
+}
+
+int allocate_channel(int channel_id)
+{
+ int i;
+ if (channel_id == 0) {
+ channel_id = allocate_new_channel();
+ }
+
+ bool has_free_slots = false;
+ for (i = 0; i < MAX_CHANNEL; i++) {
+ if (channel_states[i].state == STATE_CHANNEL_AVAILABLE) {
+ has_free_slots = true;
+ break;
+ }
+ }
+ if (!has_free_slots)
+ cleanup_timeout();
+
+ for (i = 0; i < MAX_CHANNEL; i++) {
+ ch_state& c = channel_states[i];
+ if (c.state == STATE_CHANNEL_AVAILABLE) {
+ c.cid = channel_id;
+ c.state = STATE_CHANNEL_WAIT_PACKET;
+ c.last_millis = millis();
+ return channel_id;
+ }
+ }
+ return 0;
+}
+
+int initResponse(byte* buffer)
+{
+#ifdef DEBUG
+ Serial.print("INIT RESPONSE");
+#endif
+ int cid = *(int*)buffer;
+#ifdef DEBUG
+ Serial.print(cid, HEX);
+#endif
+ int len = buffer[5] << 8 | buffer[6];
+ int i;
+ memcpy(resp_buffer, buffer, 5);
+ SET_MSG_LEN(resp_buffer, 17);
+ memcpy(resp_buffer + 7, buffer + 7, len); // nonce
+ i = 7 + len;
+ if (cid == -1) {
+ cid = allocate_channel(0);
+ } else {
+#ifdef DEBUG
+ Serial.println("using existingg CID");
+#endif
+ allocate_channel(cid);
+ }
+ memcpy(resp_buffer + i, &cid, 4);
+ i += 4;
+ resp_buffer[i++] = U2FHID_IF_VERSION;
+ resp_buffer[i++] = 1; // major
+ resp_buffer[i++] = 0;
+ resp_buffer[i++] = 1; // build
+ // resp_buffer[i++] = CAPABILITY_WINK; //capabilities
+ resp_buffer[i++] = 0; // capabilities
+#ifdef DEBUG
+ Serial.println("SEND RESPONSE 1");
+#endif
+ RawHID.send(resp_buffer, 100);
+#ifdef DEBUG
+ Serial.println(cid, HEX);
+#endif
+ return cid;
+}
+
+void errorResponse(byte* buffer, int code)
+{
+ memcpy(resp_buffer, buffer, 4);
+ resp_buffer[4] = U2FHID_ERROR;
+ SET_MSG_LEN(resp_buffer, 1);
+ resp_buffer[7] = code & 0xff;
+#ifdef DEBUG
+ Serial.print("SENT RESPONSE error");
+ Serial.println(code);
+#endif
+ RawHID.send(resp_buffer, 100);
+}
+
+// find channel index and update last access
+int find_channel_index(int channel_id)
+{
+ int i;
+
+ for (i = 0; i < MAX_CHANNEL; i++) {
+ if (channel_states[i].cid == channel_id) {
+ channel_states[i].last_millis = millis();
+ return i;
+ }
+ }
+
+ return -1;
+}
+
+void respondErrorPDU(byte* buffer, int err)
+{
+ SET_MSG_LEN(buffer, 2); // len("") + 2 byte SW
+ byte* datapart = buffer + 7;
+ APPEND_SW(datapart, (err >> 8) & 0xff, err & 0xff);
+ RawHID.send(buffer, 100);
+}
+
+void sendLargeResponse(byte* request, int len)
+{
+#ifdef DEBUG
+ Serial.print("Sending large response ");
+ Serial.println(len);
+ for (int i = 0; i < len; i++) {
+ Serial.print(large_resp_buffer[i], HEX);
+ Serial.print(" ");
+ }
+ Serial.println("\n--\n");
+#endif
+
+ memcpy(resp_buffer, request, 4); // copy cid
+ resp_buffer[4] = U2FHID_MSG;
+ int r = len;
+ if (r > MAX_INITIAL_PACKET) {
+ r = MAX_INITIAL_PACKET;
+ }
+
+ SET_MSG_LEN(resp_buffer, len);
+ memcpy(resp_buffer + 7, large_resp_buffer, r);
+
+ RawHID.send(resp_buffer, 100);
+ len -= r;
+ byte p = 0;
+ int offset = MAX_INITIAL_PACKET;
+ while (len > 0) {
+ // memcpy(resp_buffer, request, 4); //copy cid, doesn't need to recopy
+ resp_buffer[4] = p++;
+ memcpy(resp_buffer + 5, large_resp_buffer + offset,
+ MAX_CONTINUATION_PACKET);
+ RawHID.send(resp_buffer, 100);
+ len -= MAX_CONTINUATION_PACKET;
+ offset += MAX_CONTINUATION_PACKET;
+ delayMicroseconds(2500);
+ }
+}
+
+int getCounter()
+{
+ unsigned int eeAddress = 0; // EEPROM address to start reading from
+ unsigned int counter;
+ EEPROM.get(eeAddress, counter);
+ return counter;
+}
+
+void setCounter(int counter)
+{
+ unsigned int eeAddress = 0; // EEPROM address to start reading from
+ EEPROM.put(eeAddress, counter);
+}
+
+void processMessage(byte* buffer)
+{
+ int len = buffer[5] << 8 | buffer[6];
+#ifdef DEBUG
+ Serial.println(F("Got message"));
+ Serial.println(len);
+ Serial.println(F("Data:"));
+#endif
+
+ byte* message = buffer + 7;
+
+#ifdef DEBUG
+ for (int i = 7; i < 7 + len; i++) {
+ Serial.print(buffer[i], HEX);
+ }
+ Serial.println(F(""));
+#endif
+ byte CLA = message[0];
+
+#ifdef DEBUG
+ Serial.print("CLA : ");
+ Serial.println(CLA, HEX);
+#endif
+ if (CLA != 0) {
+ respondErrorPDU(buffer, SW_CLA_NOT_SUPPORTED);
+ return;
+ }
+
+ byte INS = message[1];
+#ifdef DEBUG
+ Serial.print("INS : ");
+ Serial.println(INS, HEX);
+#endif
+ byte P1 = message[2];
+ // byte P2 = message[3];
+ int reqlength = (message[4] << 16) | (message[5] << 8) | message[6];
+
+ switch (INS) {
+ case U2F_INS_REGISTER: {
+ if (reqlength != 64) {
+ respondErrorPDU(buffer, SW_WRONG_LENGTH);
+ return;
+ }
+
+ if (!EEPROM.read(EEPROM_KEYCLOAK_EN))
+ hash_matched = true;
+
+ if ((!button_pressed) || (!hash_matched)) {
+ respondErrorPDU(buffer, SW_CONDITIONS_NOT_SATISFIED);
+ /*Set fingerprint LED ring to always ON in blue */
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eKeepsOn,
+ /*LEDColor = */ fingerprint.eLEDBlue,
+ /*blinkCount = */ 0);
+
+ return;
+ }
+
+ byte* datapart = message + 7;
+ byte* challenge_parameter = datapart;
+ byte* application_parameter = datapart + 32;
+
+ memset(public_k, 0, sizeof(public_k));
+ memset(private_k, 0, sizeof(private_k));
+ uECC_make_key(public_k + 1, private_k, curve); // so we ca insert 0x04
+ public_k[0] = 0x04;
+#ifdef DEBUG
+ Serial.println(F("Public K"));
+ for (size_t i = 0; i < sizeof(public_k); i++) {
+ Serial.print(public_k[i], HEX);
+ Serial.print(" ");
+ }
+ Serial.println("");
+ Serial.println(F("Private K"));
+ for (size_t i = 0; i < sizeof(private_k); i++) {
+ Serial.print(private_k[i], HEX);
+ Serial.print(" ");
+ }
+ Serial.println("");
+#endif
+
+ // construct hash
+
+ memcpy(handle, application_parameter, 32);
+ memcpy(handle + 32, private_k, 32);
+ for (int i = 0; i < 64; i++) {
+ handle[i] ^= handlekey[i % (sizeof(handlekey) - 1)];
+ }
+ // prepare key
+ aes128.setKey(key, aes128.keySize());
+
+ // encrypt handle
+ encrypt_handle();
+
+ SHA256_CTX ctx;
+ sha256_init(&ctx);
+ large_resp_buffer[0] = 0x00;
+ sha256_update(&ctx, large_resp_buffer, 1);
+#ifdef DEBUG
+ Serial.println(F("App Parameter:"));
+ for (int i = 0; i < 32; i++) {
+ Serial.print(application_parameter[i], HEX);
+ Serial.print(" ");
+ }
+ Serial.println("");
+#endif
+
+ sha256_update(&ctx, application_parameter, 32);
+#ifdef DEBUG
+ Serial.println(F("Chal Parameter:"));
+ for (int i = 0; i < 32; i++) {
+ Serial.print(challenge_parameter[i], HEX);
+ Serial.print(" ");
+ }
+ Serial.println("");
+#endif
+
+ sha256_update(&ctx, challenge_parameter, 32);
+
+#ifdef DEBUG
+ Serial.println(F("Handle Parameter:"));
+ for (int i = 0; i < 64; i++) {
+ Serial.print(handle[i], HEX);
+ Serial.print(" ");
+ }
+ Serial.println("");
+#endif
+ sha256_update(&ctx, handle, 64);
+ sha256_update(&ctx, public_k, 65);
+#ifdef DEBUG
+ Serial.println(F("Public key:"));
+ for (int i = 0; i < 65; i++) {
+ Serial.print(public_k[i], HEX);
+ Serial.print(" ");
+ }
+ Serial.println("");
+#endif
+
+ sha256_final(&ctx, sha256_hash);
+#ifdef DEBUG
+ Serial.println(F("Hash:"));
+ for (int i = 0; i < 32; i++) {
+ Serial.print(sha256_hash[i], HEX);
+ Serial.print(" ");
+ }
+ Serial.println("");
+#endif
+ uint8_t* signature = resp_buffer; // temporary
+ uint8_t tmp[32 + 32 + 64];
+ SHA256_HashContext ectx
+ = { { &init_SHA256, &update_SHA256, &finish_SHA256, 64, 32, tmp } };
+
+ uECC_sign_deterministic((uint8_t*)attestation_key, sha256_hash, 32,
+ &ectx.uECC, signature, curve);
+
+ int len = 0;
+ large_resp_buffer[len++] = 0x05;
+ memcpy(large_resp_buffer + len, public_k, 65);
+ len += 65;
+ large_resp_buffer[len++] = 64; // length of handle
+ memcpy(large_resp_buffer + len, handle, 64);
+ len += 64;
+ memcpy(
+ large_resp_buffer + len, attestation_der, sizeof(attestation_der));
+ len += sizeof(attestation_der) - 1;
+ // convert signature format
+ // http://bitcoin.stackexchange.com/questions/12554/why-the-signature-is-always-65-13232-bytes-long
+ large_resp_buffer[len++] = 0x30; // header: compound structure
+ uint8_t* total_len = &large_resp_buffer[len];
+ large_resp_buffer[len++] = 0x44; // total length (32 + 32 + 2 + 2)
+ large_resp_buffer[len++] = 0x02; // header: integer
+
+ if (signature[0] > 0x7f) {
+ large_resp_buffer[len++] = 33; // 33 byte
+ large_resp_buffer[len++] = 0;
+ (*total_len)++; // update total length
+ } else {
+ large_resp_buffer[len++] = 32; // 32 byte
+ }
+
+ memcpy(large_resp_buffer + len, signature, 32); // R value
+ len += 32;
+ large_resp_buffer[len++] = 0x02; // header: integer
+
+ if (signature[32] > 0x7f) {
+ large_resp_buffer[len++] = 33; // 32 byte
+ large_resp_buffer[len++] = 0;
+ (*total_len)++; // update total length
+ } else {
+ large_resp_buffer[len++] = 32; // 32 byte
+ }
+
+ memcpy(large_resp_buffer + len, signature + 32, 32); // R value
+ len += 32;
+
+ byte* last = large_resp_buffer + len;
+ APPEND_SW_NO_ERROR(last);
+ len += 2;
+ sendLargeResponse(buffer, len);
+ button_pressed = false;
+ hash_matched = false;
+ /*Set fingerprint LED ring to always ON in blue */
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eKeepsOn,
+ /*LEDColor = */ fingerprint.eLEDBlue,
+ /*blinkCount = */ 0);
+
+ }
+
+ break;
+ case U2F_INS_AUTHENTICATE: {
+
+ // minimum is 64 + 1 + 64
+ if (reqlength != (64 + 1 + 64)) {
+ respondErrorPDU(buffer, SW_WRONG_LENGTH);
+ return;
+ }
+
+ if (!EEPROM.read(EEPROM_KEYCLOAK_EN))
+ hash_matched = true;
+ if ((!button_pressed) || (!hash_matched)) {
+ respondErrorPDU(buffer, SW_CONDITIONS_NOT_SATISFIED);
+ return;
+ /*Set fingerprint LED ring to always ON in blue */
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eKeepsOn,
+ /*LEDColor = */ fingerprint.eLEDBlue,
+ /*blinkCount = */ 0);
+ }
+
+ byte* datapart = message + 7;
+ byte* challenge_parameter = datapart;
+ byte* application_parameter = datapart + 32;
+ byte handle_len = datapart[64];
+ byte* client_handle = datapart + 65;
+
+ if (handle_len != 64) {
+ // not from this device
+ respondErrorPDU(buffer, SW_WRONG_DATA);
+ return;
+ }
+
+ memcpy(handle, client_handle, 64);
+
+ // Prepare key
+ aes128.setKey(key, aes128.keySize());
+ // decrypt handle using AES-128
+ decrypt_handle();
+
+ for (int i = 0; i < 64; i++) {
+ handle[i] ^= handlekey[i % (sizeof(handlekey) - 1)];
+ }
+#ifdef DEBUG
+ Serial.println("handle : ");
+ for (uint8_t i = 0; i < sizeof(handle); i++) {
+ Serial.print(handle[i], HEX);
+ }
+ Serial.println("");
+#endif
+ uint8_t* key = handle + 32;
+
+ if (memcmp(handle, application_parameter, 32) != 0) {
+ // this handle is not from us
+ respondErrorPDU(buffer, SW_WRONG_DATA);
+ return;
+ }
+
+ if (P1 == 0x07) { // check-only
+ respondErrorPDU(buffer, SW_CONDITIONS_NOT_SATISFIED);
+ } else if (P1 == 0x03) { // enforce-user-presence-and-sign
+ int counter = getCounter();
+ SHA256_CTX ctx;
+ sha256_init(&ctx);
+ sha256_update(&ctx, application_parameter, 32);
+ large_resp_buffer[0] = 0x01; // user_presence
+
+ int ctr = ((counter >> 24) & 0xff) | // move byte 3 to byte 0
+ ((counter << 8) & 0xff0000) | // move byte 1 to byte 2
+ ((counter >> 8) & 0xff00) | // move byte 2 to byte 1
+ ((counter << 24) & 0xff000000); // byte 0 to byte 3
+
+ memcpy(large_resp_buffer + 1, &ctr, 4);
+
+ sha256_update(&ctx, large_resp_buffer, 5); // user presence + ctr
+
+ sha256_update(&ctx, challenge_parameter, 32);
+ sha256_final(&ctx, sha256_hash);
+
+ uint8_t* signature = resp_buffer; // temporary
+
+ uint8_t tmp[32 + 32 + 64];
+ SHA256_HashContext ectx = { { &init_SHA256, &update_SHA256,
+ &finish_SHA256, 64, 32, tmp } };
+
+ uECC_sign_deterministic(
+ (uint8_t*)key, sha256_hash, 32, &ectx.uECC, signature, curve);
+
+ int len = 5;
+
+ // convert signature format
+ // http://bitcoin.stackexchange.com/questions/12554/why-the-signature-is-always-65-13232-bytes-long
+ large_resp_buffer[len++] = 0x30; // header: compound structure
+ uint8_t* total_len = &large_resp_buffer[len];
+ large_resp_buffer[len++] = 0x44; // total length (32 + 32 + 2 + 2)
+ large_resp_buffer[len++] = 0x02; // header: integer
+
+ if (signature[0] > 0x7f) {
+ large_resp_buffer[len++] = 33; // 33 byte
+ large_resp_buffer[len++] = 0;
+ (*total_len)++; // update total length
+ } else {
+ large_resp_buffer[len++] = 32; // 32 byte
+ }
+
+ memcpy(large_resp_buffer + len, signature, 32); // R value
+ len += 32;
+ large_resp_buffer[len++] = 0x02; // header: integer
+
+ if (signature[32] > 0x7f) {
+ large_resp_buffer[len++] = 33; // 32 byte
+ large_resp_buffer[len++] = 0;
+ (*total_len)++; // update total length
+ } else {
+ large_resp_buffer[len++] = 32; // 32 byte
+ }
+
+ button_pressed = false;
+ hash_matched = false;
+ /*Set fingerprint LED ring to always ON in blue */
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eKeepsOn,
+ /*LEDColor = */ fingerprint.eLEDBlue,
+ /*blinkCount = */ 0);
+
+ memcpy(large_resp_buffer + len, signature + 32, 32); // R value
+ len += 32;
+ byte* last = large_resp_buffer + len;
+ APPEND_SW_NO_ERROR(last);
+ len += 2;
+#ifdef DEBUG
+ Serial.print("Len to send ");
+ Serial.println(len);
+#endif
+
+ sendLargeResponse(buffer, len);
+
+ setCounter(counter + 1);
+ } else {
+ // return error
+ }
+
+ } break;
+ case U2F_INS_VERSION: {
+ if (reqlength != 0) {
+ respondErrorPDU(buffer, SW_WRONG_LENGTH);
+ return;
+ }
+ // reuse input buffer for sending
+ SET_MSG_LEN(buffer, 8); // len("U2F_V2") + 2 byte SW
+ byte* datapart = buffer + 7;
+ memcpy(datapart, "U2F_V2", 6);
+ datapart += 6;
+ APPEND_SW_NO_ERROR(datapart);
+ RawHID.send(buffer, 100);
+ } break;
+
+ case U2F_INS_FPHASH: {
+ Serial.println("Getting FPHASH");
+ byte buf[64] = {};
+ buf[0] = 0x69;
+
+ if (LAST_MATCHED_ID < 1) {
+ Serial.printf("LAST_ID: %d\n", LAST_MATCHED_ID);
+ RawHID.send(buf, 100);
+ return;
+ }
+ Serial.printf("Getting hash for ID: %d\n", LAST_MATCHED_ID - 1);
+ uint16_t memoryAddress = EEPROM_FP_HASH + ((LAST_MATCHED_ID - 1) * 32);
+
+ uint8_t idx = 1;
+ for (uint16_t i = memoryAddress; i < (memoryAddress + SHA256_SIZE);
+ i++) {
+ buf[idx] = EEPROM.read(i);
+ Serial.printf("%d ", buf[idx]);
+ idx++;
+ }
+ Serial.printf("\n");
+ RawHID.send(buf, 100);
+ } break;
+
+ case U2F_INS_HASHMATCH: {
+ /*
+byte hash_buffer[64];
+for (uint8_t i = 0; i < 64; i++) {
+hash_buffer[i] = 0;
+}
+RawHID.send(hash_buffer, 100);
+ */
+ hash_matched = true;
+ } break;
+
+ case U2F_INS_CLR_FPS: {
+
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eBreathing,
+ /*LEDColor = */ fingerprint.eLEDBlue,
+ /*blinkCount = */ 0);
+ fingerprint.delFingerprint(DELALL); // delete all fingerprint
+ Serial.println("All fingerprints cleared");
+ Serial.println("-----------------------------");
+ button_pressed = false;
+ hash_matched = false;
+ delay(1000);
+ } break;
+ case U2F_INS_DEL_FPS: {
+ while (true) {
+ uint8_t ret = 0;
+ Serial.println(
+ "Press your finger on the sensor to delete the fingerprint");
+
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eBreathing,
+ /*LEDColor = */ fingerprint.eLEDBlue, /*blinkCount = */ 0);
+
+ uint8_t buf[64] = { 0 };
+ buf[0] = 0x69;
+ if ((fingerprint.collectionFingerprint(/*timeout=*/10))
+ != ERR_ID809) {
+
+ ret = fingerprint.search();
+ if (ret != 0) {
+ /*Delete the fingerprint of this ID*/
+ fingerprint.delFingerprint(/*Fingerprint ID = */ ret);
+ // fingerprint.delFingerprint(DELALL); //Delete all
+ // fingerprints
+ Serial.print("delete succeeds,ID=");
+ Serial.println(ret);
+ /*Set fingerprint LED ring to always ON in green */
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eKeepsOn,
+ /*LEDColor = */ fingerprint.eLEDGreen,
+ /*blinkCount = */ 0);
+ buf[1] = 0xac;
+ RawHID.send(buf, 100);
+ button_pressed = false;
+ hash_matched = false;
+ return;
+ } else {
+ Serial.println("Fingerprint is unregistered");
+ /*Set fingerprint LED ring to always ON in red*/
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eKeepsOn,
+ /*LEDColor = */ fingerprint.eLEDRed,
+ /*blinkCount = */ 0);
+ buf[1] = 0xaf;
+ RawHID.send(buf, 100);
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eBreathing,
+ /*LEDColor = */ fingerprint.eLEDBlue,
+ /*blinkCount = */ 0);
+ button_pressed = false;
+ hash_matched = false;
+ return;
+ }
+ } else {
+ Serial.println("Capturing fails");
+ /*Get error code information*/
+ // desc = fingerprint.getErrorDescription();
+ // Serial.println(desc);
+ /*Set fingerprint LED ring to always ON in red*/
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eKeepsOn,
+ /*LEDColor = */ fingerprint.eLEDRed, /*blinkCount = */ 0);
+ // buf[1] = 0xaf;
+ // RawHID.send(buf, 100);
+ // return;
+ }
+ Serial.println("Please release your finger");
+ /*Wait for finger to release
+ Return 1 when finger is detected, otherwise return 0
+ */
+ while (fingerprint.detectFinger())
+ ;
+ delay(1000);
+ }
+ break;
+ } break;
+
+ case U2F_INS_REG_FPS: {
+
+ uint8_t i = 0;
+ /*Get an unregistered ID for saving fingerprint
+ Return ID when succeeded
+ Return ERR_ID809 if failed
+ */
+ if ((ID = fingerprint.getEmptyID()) == ERR_ID809) {
+ while (1) {
+ /*Get error code information*/
+ // desc = fingerprint.getErrorDescription();
+ // Serial.println(desc);
+ delay(1000);
+ }
+ }
+ Serial.print("unresistered ID,ID=");
+ Serial.println(ID);
+
+ uint8_t buf[64] = { 0 };
+ buf[0] = 0x69;
+ buf[1] = 0xab;
+
+ i = 0; // Clear sampling times
+ /*Fingerprint sampling 3 times*/
+ while (i < COLLECT_NUMBER) {
+ /*Set fingerprint LED ring mode, color, and number of blinks
+ Can be set as follows:
+ Parameter 1:<LEDMode>
+ eBreathing eFastBlink eKeepsOn eNormalClose
+ eFadeIn eFadeOut eSlowBlink
+ Parameter 2:<LEDColor>
+ eLEDGreen eLEDRed eLEDYellow eLEDBlue
+ eLEDCyan eLEDMagenta eLEDWhite
+ Parameter 3:<Number of blinks> 0 represents blinking all the time
+ This parameter will only be valid in mode eBreathing, eFastBlink,
+ eSlowBlink
+ */
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eBreathing,
+ /*LEDColor = */ fingerprint.eLEDBlue,
+ /*blinkCount = */ 0);
+ Serial.print("The fingerprint sampling of the");
+ Serial.print(i + 1);
+ Serial.println("(th) is being taken");
+ Serial.println("Please press down your finger");
+ /*Capture fingerprint image, 10s idle timeout, if timeout=0,Disable
+ the collection timeout function IF succeeded, return 0, otherwise,
+ return ERR_ID809
+ */
+ if ((fingerprint.collectionFingerprint(/*timeout = */ 10))
+ != ERR_ID809) {
+ /*Set fingerprint LED ring to quick blink in yellow 3 times */
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eFastBlink,
+ /*LEDColor = */ fingerprint.eLEDYellow,
+ /*blinkCount = */ 3);
+ Serial.println("Sampling succeeds");
+ i++; // Sampling times +1
+ uint8_t buf[64] = { 0 };
+ buf[0] = 0x69;
+ buf[1] = 0x4f;
+ RawHID.send(buf, 100);
+ } else {
+ Serial.println("Sampling failed");
+ /*Get error code information*/
+ // desc = fingerprint.getErrorDescription();
+ // Serial.println(desc);
+ }
+ Serial.println("Please release your finger");
+ /*Wait for finger to release
+ Return 1 when finger is detected, otherwise return 0
+ */
+ while (fingerprint.detectFinger())
+ ;
+ }
+
+ if (ID < 1) {
+ Serial.println("Fingerprint registration failed");
+ return;
+ }
+
+ /*Save fingerprint in an unregistered ID */
+ if (fingerprint.storeFingerprint(/*Empty ID = */ ID) != ERR_ID809) {
+ Serial.print("Saving succeed，ID=");
+ Serial.println(ID);
+ Serial.println("-----------------------------");
+ /*Set fingerprint LED ring to always ON in green */
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eKeepsOn,
+ /*LEDColor = */ fingerprint.eLEDGreen,
+ /*blinkCount = */ 0);
+ delay(1000);
+ /*Turn off fingerprint LED ring */
+ fingerprint.ctrlLED(/*LEDMode = */ fingerprint.eNormalClose,
+ /*LEDColor = */ fingerprint.eLEDBlue,
+ /*blinkCount = */ 0);
+ delay(1000);
+
+ fingerprint.getTemplate(ID, temp);
+ compute_hash();
+ saveHash();
+ readHash();
+
+ uint8_t buf[64] = { 0 };
+ buf[0] = 0x69;
+ buf[1] = 0x5f;
+ buf[2] = ID;
+
+ memcpy(&buf[3], HASH_DATA, 32);
+
+ // send the hash along with it
+
+ RawHID.send(buf, 100);
+
+ } else {
+ uint8_t buf[64] = { 0 };
+ buf[0] = 0x69;
+ buf[1] = 0x6f;
+ RawHID.send(buf, 100);
+
+ Serial.println("Saving failed");
+ /*Get error code information*/
+ // desc = fingerprint.getErrorDescription();
+ // Serial.println(desc);
+ }
+
+ byte hash_buffer[64];
+ for (uint8_t i = 0; i < 64; i++) {
+
+ hash_buffer[i] = 0;
+ }
+ RawHID.send(hash_buffer, 100);
+ button_pressed = false;
+ hash_matched = false;
+
+ } break;
+ case U2F_INS_PING: {
+ Serial.printf("Got a ping msg");
+ uint8_t buf[64] = { 0 };
+
+ buf[0] = 0x69;
+ buf[1] = 0x11;
+ buf[2] = fingerprint.getEnrollCount();
+ memcpy(&buf[3], U_ID, 32);
+ RawHID.send(buf, 100);
+ } break;
+ case U2F_INS_KEYCLOAK_EN: {
+ Serial.printf("Enable keyCloak hash match");
+
+ EEPROM.write(EEPROM_KEYCLOAK_EN, KEYCLOAK_EN);
+
+ uint8_t buf[64] = { 0 };
+ RawHID.send(buf, 100);
+ } break;
+ case U2F_INS_KEYCLOAK_DIS: {
+ Serial.printf("Disable keyCloak hash match");
+
+ EEPROM.write(EEPROM_KEYCLOAK_EN, KEYCLOAK_DIS);
+
+ uint8_t buf[64] = { 0 };
+ RawHID.send(buf, 100);
+ } break;
+
+ default: {
+ respondErrorPDU(buffer, SW_INS_NOT_SUPPORTED);
+ };
+ }
+}
+
+void processPacket(byte* buffer)
+{
+#ifdef DEBUG
+ Serial.print("Process CMD ");
+#endif
+ unsigned char cmd = buffer[4]; // cmd or continuation
+#ifdef DEBUG
+ Serial.println((int)cmd, HEX);
+#endif
+ int len = buffer[5] << 8 | buffer[6];
+ if (cmd > U2FHID_INIT || cmd == U2FHID_LOCK) {
+ errorResponse(recv_buffer, ERR_INVALID_CMD);
+ return;
+ }
+ if (cmd == U2FHID_PING) {
+ if (len <= MAX_INITIAL_PACKET) {
+#ifdef DEBUG
+ Serial.println("Sending ping response");
+#endif
+ RawHID.send(buffer, 100);
+ } else {
+ // large packet
+ // send first one
+#ifdef DEBUG
+ Serial.println("SENT RESPONSE 3");
+#endif
+ RawHID.send(buffer, 100);
+ len -= MAX_INITIAL_PACKET;
+ byte p = 0;
+ int offset = 7 + MAX_INITIAL_PACKET;
+ while (len > 0) {
+ memcpy(resp_buffer, buffer, 4); // copy cid
+ resp_buffer[4] = p++;
+ memcpy(
+ resp_buffer + 5, buffer + offset, MAX_CONTINUATION_PACKET);
+ RawHID.send(resp_buffer, 100);
+ len -= MAX_CONTINUATION_PACKET;
+ offset += MAX_CONTINUATION_PACKET;
+ delayMicroseconds(2500);
+
+#ifdef DEBUG
+ Serial.println("Sending large ping response");
+#endif
+ }
+ }
+ }
+ if (cmd == U2FHID_MSG) {
+ processMessage(buffer);
+ }
+}
+
+void setOtherTimeout()
+{
+ // we can process the data
+ // but if we find another channel is waiting for continuation, we set it as
+ // timeout
+ for (int i = 0; i < MAX_CHANNEL; i++) {
+ if (channel_states[i].state == STATE_CHANNEL_WAIT_CONT) {
+#ifdef DEBUG
+ Serial.println("Set other timeout");
+#endif
+ channel_states[i].state = STATE_CHANNEL_TIMEOUT;
+ }
+ }
+}
+
+int cont_start = 0;
+
+void authenticateKey()
+{
+ int n;
+ int cid_vendor = CID_VENDOR;
+
+ n = RawHID.recv(recv_buffer, 0); // 0 timeout = do not wait
+
+ if (n > 0) {
+
+#ifdef DEBUG
+ Serial.print(F("\n\nReceived packet, CID: "));
+#endif
+ // int cid = *(int*)recv_buffer;
+ int cid; // handle strict-aliasing warning
+ memcpy(&cid, recv_buffer, sizeof(cid));
+#ifdef DEBUG
+ Serial.println(cid, HEX);
+#endif
+
+ if ((cid == cid_vendor) && (recv_buffer[4] != 0xFF)) {
+ processMessage(recv_buffer);
+ return;
+ }
+ if (cid == 0) {
+ errorResponse(recv_buffer, ERR_INVALID_CID);
+ return;
+ }
+
+ unsigned char cmd_or_cont = recv_buffer[4]; // cmd or continuation
+
+ int len = (recv_buffer[5]) << 8 | recv_buffer[6];
+
+#ifdef DEBUG
+ if (IS_NOT_CONTINUATION_PACKET(cmd_or_cont)) {
+ Serial.print(F("LEN "));
+ Serial.println((int)len);
+ }
+#endif
+ // don't care about cid
+ if (cmd_or_cont == U2FHID_INIT) {
+ setOtherTimeout();
+ cid = initResponse(recv_buffer);
+ int cidx = find_channel_index(cid);
+ channel_states[cidx].state = STATE_CHANNEL_WAIT_PACKET;
+ return;
+ }
+
+ if (cid == -1) {
+ errorResponse(recv_buffer, ERR_INVALID_CID);
+ return;
+ }
+
+ int cidx = find_channel_index(cid);
+
+ if (cidx == -1) {
+
+#ifdef DEBUG
+ Serial.println("allocating new CID");
+#endif
+ allocate_channel(cid);
+ cidx = find_channel_index(cid);
+ if (cidx == -1) {
+ errorResponse(recv_buffer, ERR_INVALID_CID);
+ return;
+ }
+ }
+
+ if (IS_NOT_CONTINUATION_PACKET(cmd_or_cont)) {
+
+ if (len > MAX_TOTAL_PACKET) {
+ errorResponse(recv_buffer, ERR_INVALID_LEN); // invalid length
+ return;
+ }
+
+ if (len > MAX_INITIAL_PACKET) {
+ // if another channel is waiting for continuation, we respond
+ // with busy
+ for (int i = 0; i < MAX_CHANNEL; i++) {
+ if (channel_states[i].state == STATE_CHANNEL_WAIT_CONT) {
+ if (i == cidx) {
+ errorResponse(recv_buffer,
+ ERR_INVALID_SEQ); // invalid sequence
+ channel_states[i].state = STATE_CHANNEL_WAIT_PACKET;
+ } else {
+ errorResponse(recv_buffer, ERR_CHANNEL_BUSY);
+ return;
+ }
+
+ return;
+ }
+ }
+ // no other channel is waiting
+ channel_states[cidx].state = STATE_CHANNEL_WAIT_CONT;
+ cont_start = millis();
+ memcpy(large_buffer, recv_buffer, 64);
+ large_data_len = len;
+ large_data_offset = MAX_INITIAL_PACKET;
+ expected_next_packet = 0;
+ return;
+ }
+
+ setOtherTimeout();
+ processPacket(recv_buffer);
+ channel_states[cidx].state = STATE_CHANNEL_WAIT_PACKET;
+ } else {
+
+ if (channel_states[cidx].state != STATE_CHANNEL_WAIT_CONT) {
+
+#ifdef DEBUG
+ Serial.println("ignoring stray packet");
+ Serial.println(cid, HEX);
+#endif
+ return;
+ }
+
+ // this is a continuation
+ if (cmd_or_cont != expected_next_packet) {
+ errorResponse(recv_buffer, ERR_INVALID_SEQ); // invalid sequence
+ channel_states[cidx].state = STATE_CHANNEL_WAIT_PACKET;
+ return;
+ } else {
+
+ memcpy(large_buffer + large_data_offset + 7, recv_buffer + 5,
+ MAX_CONTINUATION_PACKET);
+ large_data_offset += MAX_CONTINUATION_PACKET;
+
+ if (large_data_offset < large_data_len) {
+ expected_next_packet++;
+#ifdef DEBUG
+ Serial.println("Expecting next cont");
+#endif
+
+ return;
+ }
+#ifdef DEBUG
+ Serial.println("Completed");
+#endif
+ channel_states[cidx].state = STATE_CHANNEL_WAIT_PACKET;
+ processPacket(large_buffer);
+ return;
+ }
+ }
+ } else {
+
+ for (int i = 0; i < MAX_CHANNEL; i++) {
+ if (channel_states[i].state == STATE_CHANNEL_TIMEOUT) {
+#ifdef DEBUG
+ Serial.println("send timeout");
+ Serial.println(channel_states[i].cid, HEX);
+#endif
+
+ memcpy(recv_buffer, &channel_states[i].cid, 4);
+ errorResponse(recv_buffer, ERR_MSG_TIMEOUT);
+ channel_states[i].state = STATE_CHANNEL_WAIT_PACKET;
+ }
+ if (channel_states[i].state == STATE_CHANNEL_WAIT_CONT) {
+
+ int now = millis();
+ if ((now - channel_states[i].last_millis) > 500) {
+#ifdef DEBUG
+ Serial.println("SET timeout");
+#endif
+ channel_states[i].state = STATE_CHANNEL_TIMEOUT;
+ }
+ }
+ }
+ }
+}
+
+void loop()
+{
+ auto count = fingerprint.getEnrollCount();
+ if (!button_pressed && count != 0) {
+ readFingerPrint();
+ // authenticateKey();
+ } else {
+ authenticateKey();
+ }
+}
--
GitLab