diff --git a/.gitignore b/.gitignore index 2ddf7a3..3698563 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,9 @@ -deleteme SOut.txt comdev.txt comhost.txt +libuECC.o +monitor +obj/* +U2F_Priv_Keys.txt +devpackets.txt +hostpackets.txt diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..aa2f181 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "micro-ecc"] + path = micro-ecc + url = https://github.com/kmackay/micro-ecc.git diff --git a/APDU.hpp b/APDU.hpp new file mode 100644 index 0000000..f0f8f76 --- /dev/null +++ b/APDU.hpp @@ -0,0 +1,8 @@ +#pragma once + +enum APDU +{ + U2F_REG = 0x01, + U2F_AUTH = 0x02, + U2F_VER = 0x03 +}; diff --git a/Base64.hpp b/Base64.hpp new file mode 100644 index 0000000..ea61aeb --- /dev/null +++ b/Base64.hpp @@ -0,0 +1,19 @@ +#pragma once + +template +void b64encode(const InContainer &iContainer, OutContainer &oContainer); + +template +void b64decode(const InContainer &container, OutContainer &oContainer); + +template +void b64encode(const InContainer &iContainer, std::array &oArr); + +template +void b64decode(const InContainer &iContainer, std::array &oArr); + +template +void b64encode(const InContainerIter beginIter, const InContainerIter endIter, OutContainerIter oBeginIter); + +template +void b64decode(const InContainerIter beginIter, const InContainerIter endIter, OutContainerIter oBeginIter); diff --git a/Base64.tpp b/Base64.tpp new file mode 100644 index 0000000..8c76097 --- /dev/null +++ b/Base64.tpp @@ -0,0 +1,53 @@ +#include "Base64.hpp" +#include +#include +#include +#include +#include + +template +void b64encode(const InContainer &iContainer, OutContainer &oContainer) +{ + b64encode(std::begin(iContainer), std::end(iContainer), std::back_inserter(oContainer)); +} + +template +void b64decode(const InContainer &iContainer, OutContainer &oContainer) +{ + b64decode(std::begin(iContainer), std::end(iContainer), std::back_inserter(oContainer)); +} + +template +void b64encode(const InContainer &iContainer, std::array &oArr) +{ + b64encode(std::begin(iContainer), std::end(iContainer), std::begin(oArr)); +} + +template +void b64decode(const InContainer &iContainer, std::array &oArr) +{ + b64decode(std::begin(iContainer), std::end(iContainer), std::begin(oArr)); +} + +template +void b64encode(const InContainerIter beginIter, const InContainerIter endIter, OutContainerIter oBeginIter) +{ + std::stringstream oss, iss{ std::string{ beginIter, endIter } }; + base64::encoder encoder{}; + encoder.encode(iss, oss); + + auto oStr = oss.str(); + oStr.erase(std::remove_if(oStr.begin(), oStr.end(), [](auto charac){ return charac == '\n'; }), oStr.end()); + copy(oStr.begin(), oStr.end(), oBeginIter); +} + +template +void b64decode(const InContainerIter beginIter, const InContainerIter endIter, OutContainerIter oBeginIter) +{ + std::stringstream oss, iss{ std::string{ beginIter, endIter } }; + base64::decoder decoder{}; + decoder.decode(iss, oss); + + auto oStr = oss.str(); + copy(oStr.begin(), oStr.end(), oBeginIter); +} diff --git a/Certificates.cpp b/Certificates.cpp new file mode 100644 index 0000000..63b75b5 --- /dev/null +++ b/Certificates.cpp @@ -0,0 +1,72 @@ +#include "Certificates.hpp" + +//You may not actually want to use these values - +// having been shared publicly, these may be vulnerable to exploits. +// However, generating your own attestation certificate makes your device +// uniquely identifiable across platforms / services, etc. +// You can generate your own using the method detailed in the README. + +uint8_t attestCert[] = { + 0x30, 0x82, 0x02, 0x29, 0x30, 0x82, 0x01, 0xd0, 0xa0, 0x03, 0x02, 0x01, + 0x02, 0x02, 0x09, 0x00, 0x8a, 0xe2, 0x21, 0x3f, 0x2f, 0x8b, 0x72, 0x52, + 0x30, 0x0a, 0x06, 0x08, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x04, 0x03, 0x02, + 0x30, 0x70, 0x31, 0x0b, 0x30, 0x09, 0x06, 0x03, 0x55, 0x04, 0x06, 0x13, + 0x02, 0x55, 0x4b, 0x31, 0x13, 0x30, 0x11, 0x06, 0x03, 0x55, 0x04, 0x08, + 0x0c, 0x0a, 0x53, 0x6f, 0x6d, 0x65, 0x2d, 0x53, 0x74, 0x61, 0x74, 0x65, + 0x31, 0x21, 0x30, 0x1f, 0x06, 0x03, 0x55, 0x04, 0x0a, 0x0c, 0x18, 0x49, + 0x6e, 0x74, 0x65, 0x72, 0x6e, 0x65, 0x74, 0x20, 0x57, 0x69, 0x64, 0x67, + 0x69, 0x74, 0x73, 0x20, 0x50, 0x74, 0x79, 0x20, 0x4c, 0x74, 0x64, 0x31, + 0x29, 0x30, 0x27, 0x06, 0x03, 0x55, 0x04, 0x03, 0x0c, 0x20, 0x55, 0x32, + 0x46, 0x20, 0x4b, 0x65, 0x79, 0x20, 0x74, 0x50, 0x46, 0x53, 0x71, 0x54, + 0x71, 0x6f, 0x5a, 0x6d, 0x62, 0x37, 0x38, 0x61, 0x6a, 0x6f, 0x2f, 0x75, + 0x58, 0x50, 0x73, 0x51, 0x3d, 0x3d, 0x30, 0x1e, 0x17, 0x0d, 0x31, 0x38, + 0x30, 0x36, 0x33, 0x30, 0x31, 0x39, 0x30, 0x37, 0x35, 0x31, 0x5a, 0x17, + 0x0d, 0x32, 0x38, 0x30, 0x36, 0x32, 0x37, 0x31, 0x39, 0x30, 0x37, 0x35, + 0x31, 0x5a, 0x30, 0x70, 0x31, 0x0b, 0x30, 0x09, 0x06, 0x03, 0x55, 0x04, + 0x06, 0x13, 0x02, 0x55, 0x4b, 0x31, 0x13, 0x30, 0x11, 0x06, 0x03, 0x55, + 0x04, 0x08, 0x0c, 0x0a, 0x53, 0x6f, 0x6d, 0x65, 0x2d, 0x53, 0x74, 0x61, + 0x74, 0x65, 0x31, 0x21, 0x30, 0x1f, 0x06, 0x03, 0x55, 0x04, 0x0a, 0x0c, + 0x18, 0x49, 0x6e, 0x74, 0x65, 0x72, 0x6e, 0x65, 0x74, 0x20, 0x57, 0x69, + 0x64, 0x67, 0x69, 0x74, 0x73, 0x20, 0x50, 0x74, 0x79, 0x20, 0x4c, 0x74, + 0x64, 0x31, 0x29, 0x30, 0x27, 0x06, 0x03, 0x55, 0x04, 0x03, 0x0c, 0x20, + 0x55, 0x32, 0x46, 0x20, 0x4b, 0x65, 0x79, 0x20, 0x74, 0x50, 0x46, 0x53, + 0x71, 0x54, 0x71, 0x6f, 0x5a, 0x6d, 0x62, 0x37, 0x38, 0x61, 0x6a, 0x6f, + 0x2f, 0x75, 0x58, 0x50, 0x73, 0x51, 0x3d, 0x3d, 0x30, 0x59, 0x30, 0x13, + 0x06, 0x07, 0x2a, 0x86, 0x48, 0xce, 0x3d, 0x02, 0x01, 0x06, 0x08, 0x2a, + 0x86, 0x48, 0xce, 0x3d, 0x03, 0x01, 0x07, 0x03, 0x42, 0x00, 0x04, 0x32, + 0x41, 0xc3, 0xb8, 0x96, 0x97, 0xd8, 0x90, 0x66, 0x41, 0x88, 0x96, 0xd4, + 0x73, 0xb6, 0x37, 0xf7, 0x85, 0x29, 0xaf, 0x3b, 0x15, 0x0f, 0x83, 0x61, + 0x67, 0xea, 0xc9, 0xb2, 0xdb, 0x82, 0xb3, 0x2c, 0x99, 0x60, 0x8a, 0x98, + 0x7c, 0xd4, 0x04, 0xa0, 0x92, 0x22, 0x05, 0xaa, 0xf7, 0x7a, 0x91, 0x02, + 0x03, 0xdd, 0x15, 0x88, 0x87, 0x6a, 0x26, 0xe9, 0xee, 0xcf, 0x99, 0xb1, + 0x66, 0xc0, 0x01, 0xa3, 0x53, 0x30, 0x51, 0x30, 0x1d, 0x06, 0x03, 0x55, + 0x1d, 0x0e, 0x04, 0x16, 0x04, 0x14, 0xcf, 0x7f, 0xfa, 0x7d, 0xc4, 0x8d, + 0xba, 0x60, 0x52, 0x4c, 0xb6, 0x16, 0x2e, 0x88, 0x62, 0xc7, 0x8c, 0xfc, + 0xe0, 0x63, 0x30, 0x1f, 0x06, 0x03, 0x55, 0x1d, 0x23, 0x04, 0x18, 0x30, + 0x16, 0x80, 0x14, 0xcf, 0x7f, 0xfa, 0x7d, 0xc4, 0x8d, 0xba, 0x60, 0x52, + 0x4c, 0xb6, 0x16, 0x2e, 0x88, 0x62, 0xc7, 0x8c, 0xfc, 0xe0, 0x63, 0x30, + 0x0f, 0x06, 0x03, 0x55, 0x1d, 0x13, 0x01, 0x01, 0xff, 0x04, 0x05, 0x30, + 0x03, 0x01, 0x01, 0xff, 0x30, 0x0a, 0x06, 0x08, 0x2a, 0x86, 0x48, 0xce, + 0x3d, 0x04, 0x03, 0x02, 0x03, 0x47, 0x00, 0x30, 0x44, 0x02, 0x20, 0x72, + 0x25, 0x89, 0xc1, 0x32, 0x54, 0x66, 0xf8, 0x0e, 0x58, 0x77, 0xe3, 0xb5, + 0x62, 0x47, 0x33, 0x18, 0x5a, 0xdc, 0x28, 0x6a, 0x4a, 0x56, 0xcb, 0x58, + 0x63, 0xe3, 0xa1, 0x02, 0x6a, 0xf0, 0xd8, 0x02, 0x20, 0x65, 0x26, 0x84, + 0x7c, 0xc3, 0x3b, 0x7d, 0x6a, 0x22, 0x0c, 0x22, 0x3d, 0xc8, 0x43, 0xb7, + 0x84, 0x8b, 0x7b, 0x48, 0x23, 0xb0, 0x1e, 0x13, 0x35, 0x1d, 0x1a, 0x90, + 0x44, 0x62, 0x6c, 0xab, 0x9b +}; + +uint8_t attestPrivKey[] = { + 0x7e, 0xbd, 0x91, 0x05, 0x5a, 0x80, 0x9f, 0x36, 0xe5, 0x2f, 0xe0, 0xd0, + 0xa9, 0x63, 0x0c, 0x86, 0x04, 0xb1, 0x04, 0xe3, 0xd1, 0xfb, 0xd0, 0x83, + 0xc7, 0x2e, 0x2f, 0x34, 0xb6, 0xd6, 0xa4, 0xb2 +}; + +uint8_t attestPubKey[] = { + 0x04, 0x32, 0x41, 0xc3, 0xb8, 0x96, 0x97, 0xd8, 0x90, 0x66, 0x41, 0x88, + 0x96, 0xd4, 0x73, 0xb6, 0x37, 0xf7, 0x85, 0x29, 0xaf, 0x3b, 0x15, 0x0f, + 0x83, 0x61, 0x67, 0xea, 0xc9, 0xb2, 0xdb, 0x82, 0xb3, 0x2c, 0x99, 0x60, + 0x8a, 0x98, 0x7c, 0xd4, 0x04, 0xa0, 0x92, 0x22, 0x05, 0xaa, 0xf7, 0x7a, + 0x91, 0x02, 0x03, 0xdd, 0x15, 0x88, 0x87, 0x6a, 0x26, 0xe9, 0xee, 0xcf, + 0x99, 0xb1, 0x66, 0xc0, 0x01 +}; diff --git a/Certificates.hpp b/Certificates.hpp new file mode 100644 index 0000000..c489559 --- /dev/null +++ b/Certificates.hpp @@ -0,0 +1,5 @@ +#include + +extern uint8_t attestCert[557]; +extern uint8_t attestPrivKey[32]; +extern uint8_t attestPubKey[65]; diff --git a/Constants.hpp b/Constants.hpp new file mode 100644 index 0000000..e2b3d6b --- /dev/null +++ b/Constants.hpp @@ -0,0 +1,4 @@ +#pragma once +#include + +const constexpr uint16_t packetSize = 64; diff --git a/Field.hpp b/Field.hpp new file mode 100644 index 0000000..88807d4 --- /dev/null +++ b/Field.hpp @@ -0,0 +1,3 @@ +#pragma once + +#define FIELD(name) reinterpret_cast(&name), (reinterpret_cast(&name) + sizeof(name)) diff --git a/IO.cpp b/IO.cpp new file mode 100644 index 0000000..5ec855a --- /dev/null +++ b/IO.cpp @@ -0,0 +1,26 @@ +#include "IO.hpp" +#include "Streams.hpp" +#include + +using namespace std; + +vector readBytes(const size_t count) +{ + vector bytes(count); + + size_t readByteCount; + + do + { + readByteCount = fread(bytes.data(), 1, count, getHostStream().get()); + fwrite(bytes.data(), 1, bytes.size(), getComHostStream().get()); + } while (readByteCount == 0); + + clog << "Read " << readByteCount << " bytes" << endl; + + if (readByteCount != count) + throw runtime_error{ "Failed to read sufficient bytes" }; + + return bytes; +} + diff --git a/IO.hpp b/IO.hpp new file mode 100644 index 0000000..60813b1 --- /dev/null +++ b/IO.hpp @@ -0,0 +1,6 @@ +#pragma once +#include +#include +#include + +std::vector readBytes(const size_t count); diff --git a/Keys/certificate.der b/Keys/certificate.der new file mode 100644 index 0000000..3b061d1 Binary files /dev/null and b/Keys/certificate.der differ diff --git a/Keys/ecprivkey.pem b/Keys/ecprivkey.pem new file mode 100644 index 0000000..0f84abe --- /dev/null +++ b/Keys/ecprivkey.pem @@ -0,0 +1,5 @@ +-----BEGIN EC PRIVATE KEY----- +MHcCAQEEIH69kQVagJ825S/g0KljDIYEsQTj0fvQg8cuLzS21qSyoAoGCCqGSM49 +AwEHoUQDQgAEMkHDuJaX2JBmQYiW1HO2N/eFKa87FQ+DYWfqybLbgrMsmWCKmHzU +BKCSIgWq93qRAgPdFYiHaibp7s+ZsWbAAQ== +-----END EC PRIVATE KEY----- diff --git a/Keys/ecpubkey.pem b/Keys/ecpubkey.pem new file mode 100644 index 0000000..b45f686 --- /dev/null +++ b/Keys/ecpubkey.pem @@ -0,0 +1,4 @@ +-----BEGIN PUBLIC KEY----- +MFkwEwYHKoZIzj0CAQYIKoZIzj0DAQcDQgAEMkHDuJaX2JBmQYiW1HO2N/eFKa87 +FQ+DYWfqybLbgrMsmWCKmHzUBKCSIgWq93qRAgPdFYiHaibp7s+ZsWbAAQ== +-----END PUBLIC KEY----- diff --git a/Keys/privkey.der b/Keys/privkey.der new file mode 100644 index 0000000..f3dd341 --- /dev/null +++ b/Keys/privkey.der @@ -0,0 +1 @@ +~Z6/Щc Ѓ./4֤ \ No newline at end of file diff --git a/Keys/pubkey.der b/Keys/pubkey.der new file mode 100644 index 0000000..06b9bf1 --- /dev/null +++ b/Keys/pubkey.der @@ -0,0 +1 @@ +2AøؐfAs7);agɲۂ,`|"zj&ϙf \ No newline at end of file diff --git a/Keys/server.pem b/Keys/server.pem new file mode 100644 index 0000000..132699c --- /dev/null +++ b/Keys/server.pem @@ -0,0 +1,14 @@ +-----BEGIN CERTIFICATE----- +MIICKTCCAdCgAwIBAgIJAIriIT8vi3JSMAoGCCqGSM49BAMCMHAxCzAJBgNVBAYT +AlVLMRMwEQYDVQQIDApTb21lLVN0YXRlMSEwHwYDVQQKDBhJbnRlcm5ldCBXaWRn +aXRzIFB0eSBMdGQxKTAnBgNVBAMMIFUyRiBLZXkgdFBGU3FUcW9abWI3OGFqby91 +WFBzUT09MB4XDTE4MDYzMDE5MDc1MVoXDTI4MDYyNzE5MDc1MVowcDELMAkGA1UE +BhMCVUsxEzARBgNVBAgMClNvbWUtU3RhdGUxITAfBgNVBAoMGEludGVybmV0IFdp +ZGdpdHMgUHR5IEx0ZDEpMCcGA1UEAwwgVTJGIEtleSB0UEZTcVRxb1ptYjc4YWpv +L3VYUHNRPT0wWTATBgcqhkjOPQIBBggqhkjOPQMBBwNCAAQyQcO4lpfYkGZBiJbU +c7Y394UprzsVD4NhZ+rJstuCsyyZYIqYfNQEoJIiBar3epECA90ViIdqJunuz5mx +ZsABo1MwUTAdBgNVHQ4EFgQUz3/6fcSNumBSTLYWLohix4z84GMwHwYDVR0jBBgw +FoAUz3/6fcSNumBSTLYWLohix4z84GMwDwYDVR0TAQH/BAUwAwEB/zAKBggqhkjO +PQQDAgNHADBEAiByJYnBMlRm+A5Yd+O1YkczGFrcKGpKVstYY+OhAmrw2AIgZSaE +fMM7fWoiDCI9yEO3hIt7SCOwHhM1HRqQRGJsq5s= +-----END CERTIFICATE----- diff --git a/Makefile b/Makefile new file mode 100755 index 0000000..93a36f6 --- /dev/null +++ b/Makefile @@ -0,0 +1,28 @@ +#!/usr/bin/env make + +SRC_DIR := . +OBJ_DIR := obj +LDFLAGS := -lmbedcrypto -lb64 +CPPFLAGS := +CXXFLAGS := --std=c++14 + +CXXFLAGS += -MMD -MP +MODULES := $(wildcard $(SRC_DIR)/*.cpp) +OBJECTS := $(MODULES:$(SRC_DIR)/%.cpp=$(OBJ_DIR)/%.o) + +monitor: $(OBJECTS) libuECC.o + g++ $(LDFLAGS) -o $@ $^ + +$(OBJ_DIR)/%.o: $(SRC_DIR)/%.cpp + g++ $(CPPFLAGS) $(CXXFLAGS) -c -o $@ $< + +-include $(OBJECTS:.o=.d) + +clean: + rm $(OBJ_DIR)/* + rm monitor + +.PHONY: libuECC +libuECC.o: + $(MAKE) -C micro-ecc + cp micro-ecc/libuECC.o libuECC.o diff --git a/Packet.cpp b/Packet.cpp new file mode 100644 index 0000000..2710e91 --- /dev/null +++ b/Packet.cpp @@ -0,0 +1,109 @@ +#include "Packet.hpp" +#include "IO.hpp" +#include "u2f.hpp" +#include +#include +#include "Streams.hpp" + +using namespace std; + +shared_ptr InitPacket::getPacket(const uint32_t rCID, const uint8_t rCMD) +{ + auto p = make_shared(); + p->cid = rCID; + p->cmd = rCMD; + p->bcnth = readBytes(1)[0]; + p->bcntl = readBytes(1)[0]; + + const auto dataBytes = readBytes(p->data.size()); + copy(dataBytes.begin(), dataBytes.end(), p->data.begin()); + + auto hPStream = getHostPacketStream().get(); + fprintf(hPStream, "\n"); + fwrite(dataBytes.data(), 1, dataBytes.size(), hPStream); + + clog << "Fully read init packet" << endl; + return p; +} + +shared_ptr ContPacket::getPacket(const uint32_t rCID, const uint8_t rSeq) +{ + auto p = make_shared(); + p->cid = rCID; + p->seq = rSeq; + + const auto dataBytes = readBytes(p->data.size()); + copy(dataBytes.begin(), dataBytes.end(), p->data.begin()); + + auto hPStream = getHostPacketStream().get(); + fwrite(dataBytes.data(), 1, dataBytes.size(), hPStream); + + clog << "Fully read cont packet" << endl; + return p; +} + +shared_ptr Packet::getPacket() +{ + const uint32_t cid = *reinterpret_cast(readBytes(4).data()); + uint8_t b = readBytes(1)[0]; + + clog << "Packet read 2nd byte as " << static_cast(b) << endl; + + if (b & TYPE_MASK) + { + //Init packet + return InitPacket::getPacket(cid, b); + } + else + { + //Cont packet + return ContPacket::getPacket(cid, b); + } +} + +void Packet::writePacket() +{ + memset(this->buf, 0, packetSize); + memcpy(this->buf, &cid, 4); +} + +void InitPacket::writePacket() +{ + Packet::writePacket(); + auto hostStream = getHostStream().get(); + auto devStream = getComDevStream().get(); + + memcpy(this->buf + 4, &cmd, 1); + memcpy(this->buf + 5, &bcnth, 1); + memcpy(this->buf + 6, &bcntl, 1); + memcpy(this->buf + 7, data.data(), data.size()); + fwrite(this->buf, packetSize, 1, hostStream); + fwrite(this->buf, packetSize, 1, devStream); + + perror(nullptr); + + auto dPStream = getDevPacketStream().get(); + fprintf(dPStream, "\n"); + fwrite(data.data(), 1, data.size(), dPStream); + + clog << "Fully wrote init packet" << endl; +} + +void ContPacket::writePacket() +{ + Packet::writePacket(); + auto hostStream = getHostStream().get(); + auto devStream = getComDevStream().get(); + + memcpy(this->buf + 4, &seq, 1); + memcpy(this->buf + 5, data.data(), data.size()); + fwrite(this->buf, packetSize, 1, hostStream); + fwrite(this->buf, packetSize, 1, devStream); + + perror(nullptr); + + auto dPStream = getDevPacketStream().get(); + fwrite(data.data(), 1, data.size(), dPStream); + + clog << "Fully wrote cont packet" << endl; +} diff --git a/Packet.hpp b/Packet.hpp new file mode 100644 index 0000000..9a20d51 --- /dev/null +++ b/Packet.hpp @@ -0,0 +1,43 @@ +#pragma once +#include +#include +#include +#include "Constants.hpp" + +struct Packet +{ + uint32_t cid; + uint8_t buf[packetSize]; + + protected: + Packet() = default; + virtual void writePacket(); + + public: + static std::shared_ptr getPacket(); + virtual ~Packet() = default; +}; + +struct InitPacket : Packet +{ + uint8_t cmd; + uint8_t bcnth; + uint8_t bcntl; + std::array data{}; + + public: + InitPacket() = default; + static std::shared_ptr getPacket(const uint32_t rCID, const uint8_t rCMD); + void writePacket() override; +}; + +struct ContPacket : Packet +{ + uint8_t seq; + std::array data{}; + + public: + ContPacket() = default; + static std::shared_ptr getPacket(const uint32_t rCID, const uint8_t rSeq); + void writePacket() override; +}; diff --git a/Readme.AttestationCertificateGeneration.txt b/Readme.AttestationCertificateGeneration.txt new file mode 100644 index 0000000..a5be607 --- /dev/null +++ b/Readme.AttestationCertificateGeneration.txt @@ -0,0 +1,45 @@ +From https://github.com/pratikd650/Teensy_U2F/blob/master/Teensy_U2F.cpp line 292 + +Instructions to generate attestation certificate using open ssl +https://wiki.openssl.org/index.php/Command_Line_Elliptic_Curve_Operations +https://www.guyrutenberg.com/2013/12/28/creating-self-signed-ecdsa-ssl-certificate-using-openssl/ + P-256 (also secp256r1) EC key pair is W = dG (Note secp256k1 is Koblitz curve - not P256) + d = private key is it 256 bits (32 bytes) + G = generator point - it is part of the curve definition + W = public key point - it is a (256, 256) bits - 64 bytes +1) generate a key pair - the private key will be saved in PKCS8 format in ecprivkey.pem +openssl ecparam -name prime256v1 -genkey -noout -out ecprivkey.pem +2) dump out the private key in hex format - it will be a 32 byte key + openssl asn1parse -in ecprivkey.pem +3) compute the public key from the private key and the curve +openssl ec -in ecprivkey.pem -pubout -out ecpubkey.pem +4) dump out the public key in hex format - it will be 66 byte - the first two bytes are 00 04, + openssl ec -in ecprivkey.pem -pubout -text + after that is the point W - 32 byte + 32 byte +5) generate a self signed certificate + openssl req -new -x509 -key ecprivkey.pem -out server.pem -days 3650 +For the Certificate name give a unique certificate name. There is a 128 bit unique identification number burned into every +Teensy chip - see http://cache.freescale.com/files/32bit/doc/data_sheet/K20P64M72SF1.pdf +You can print out the number from your Teensy using this simple program given below +6) Display the certificate + openssl x509 -in server.pem -text -noout + +7) Convert PEM certificate to DER +openssl x509 -outform der -in server.pem -out certificate.der + +8) Generate a usable c-array for source code +xxd --include certificate.pem + +Copy output into appropriate array in 'Certificates.cpp', overwriting existing values + +9) Repeat steps 7 & 8 for public key and private key + +So: +` +openssl asn1parse -in ecprivkey.pem 2>/dev/null | grep 'HEX DUMP' | perl -pe 's/^.*\[HEX DUMP\]:(.+)$/$1/' 2>/dev/null | xxd -r -p > privkey.der && xxd --include privkey.der + +openssl ec -in ecprivkey.pem -pubout -text 2>/dev/null | perl -0777 -ne 'print /pub:.+ASN1/sg' 2>/dev/null | sed -e '/pub:/d;/ASN1/d' | perl -pe 's/^\s+(.+):?$/$1/gm' 2>/dev/null | perl -pe 's/\n//' 2>/dev/null | perl -pe 's/(.{2}):?/$1/g' 2>/dev/null | xxd -r -p > pubkey.der && xxd --include pubkey.der +` +and copy the arrays into the correct arrays in Certificates.cpp + +If any arrays have different lengths than shown in Certificates.hpp, update these too. diff --git a/Readme.txt b/Readme.txt new file mode 100644 index 0000000..35ae5c3 --- /dev/null +++ b/Readme.txt @@ -0,0 +1,8 @@ +Things to install + +//To provide HW RNG facilities +sudo apt-get install rng-tools + +//To provide ECC facilities +git submodule update --init --recursive +cp Scripts/uECC-Makefile micro-ecc/Makefile diff --git a/Scripts/uECC-Makefile b/Scripts/uECC-Makefile new file mode 100755 index 0000000..2115639 --- /dev/null +++ b/Scripts/uECC-Makefile @@ -0,0 +1,27 @@ +#!/usr/bin/env make + +SRC_DIR := . +OBJ_DIR := obj +LDFLAGS := -r +CPPFLAGS := +CXXFLAGS := --std=c++14 + +CXXFLAGS += -MMD -MP +MODULESC := $(wildcard $(SRC_DIR)/*.c) +MODULESI := $(SRC_DIR)/curve-specific.inc +OBJECTSC := $(MODULESC:$(SRC_DIR)/%.c=$(OBJ_DIR)/%.o) +OBJECTSI := $(MODULESI:$(SRC_DIR)/%.inc=$(OBJ_DIR)/%.o) +OBJECTS := $(OBJECTSC) $(OBJECTSI) + +libuECC.o: $(OBJECTS) + ld $(LDFLAGS) -o $@ $^ + +$(OBJECTSC): $(OBJ_DIR)/%.o: $(SRC_DIR)/%.c + @mkdir -p $(@D) + @g++ $(CPPFLAGS) $(CXXFLAGS) -c -o $@ $< + +$(OBJECTSI): $(OBJ_DIR)/%.o: $(SRC_DIR)/%.inc + @mkdir -p $(@D) + @g++ -x c $(CPPFLAGS) $(CXXFLAGS) -c -o $@ $< + +-include $(OBJECTS:.o=.d) diff --git a/Storage.cpp b/Storage.cpp new file mode 100644 index 0000000..45c8b4a --- /dev/null +++ b/Storage.cpp @@ -0,0 +1,90 @@ +#include "Storage.hpp" +#include +#include +#include "Base64.tpp" +#include + +using namespace std; + +std::string Storage::filename{}; +std::map Storage::appParams{}; +std::map Storage::privKeys{}; +std::map Storage::pubKeys{}; + +void Storage::init(const string &dirPrefix) +{ + Storage::filename = dirPrefix + "U2F_Priv_Keys.txt"; + ifstream file{ Storage::filename }; + string line; + size_t lineNumber = 0; + + base64::decoder decoder{}; + + while (getline(file, line)) + { + auto strLineNum = to_string(lineNumber); + stringstream ss{ line }; + string keyHStr, appStr, privStr, pubStr; + ss >> keyHStr >> appStr >> privStr >> pubStr; + + if (!ss) + throw runtime_error{ string{ "Invalid syntax of line " } + strLineNum }; + + char *endP = nullptr; + Storage::KeyHandle keyH{ strtoull(keyHStr.c_str(), &endP, 10) }; + + if (!endP) + throw runtime_error{ "Invalid keyhandle format on line " + strLineNum }; + + //if (appStr.size() != 32) + // throw runtime_error{ "Invalid length of app parameter on line " + strLineNum }; + //if (privStr.size() != 32) + // throw runtime_error{ "Invalid length of private key on line " + strLineNum }; + //if (pubStr.size() != 65) + // throw runtime_error{ "Invalid length of public key on line " + strLineNum }; + + Storage::AppParam appParam{}; + b64decode(appStr, appParam); + + Storage::PrivKey privKey{}; + b64decode(privStr, privKey); + + Storage::PubKey pubKey{}; + b64decode(pubStr, pubKey); + + Storage::appParams[keyH] = appParam; + Storage::privKeys[keyH] = privKey; + Storage::pubKeys[keyH] = pubKey; + + lineNumber++; + } +} + +void Storage::save() +{ + ofstream file{ Storage::filename }; + base64::encoder encoder{}; + + for (auto &keypair : Storage::appParams) + { + const auto keyID = keypair.first; + const auto appParam = keypair.second; + const auto privKey = Storage::privKeys[keypair.first]; + const auto pubKey = Storage::pubKeys[keypair.first]; + + file << keyID; + file << ' '; + + string appPStr{}; + b64encode(appParam, appPStr); + file << appPStr << ' '; + + string privKStr{}; + b64encode(privKey, privKStr); + file << privKStr << ' '; + + string pubKStr{}; + b64encode(pubKey, pubKStr); + file << pubKStr << endl; + } +} diff --git a/Storage.hpp b/Storage.hpp new file mode 100644 index 0000000..bf0703d --- /dev/null +++ b/Storage.hpp @@ -0,0 +1,25 @@ +#include +#include +#include +#include + +class Storage +{ +public: + using KeyHandle = uint32_t; + using AppParam = std::array; + using PrivKey = std::array; + using PubKey = std::array; + + protected: + Storage() = default; + + static std::string filename; + + public: + static void init(const std::string &dirPrefix = ""); + static void save(); + static std::map appParams; + static std::map privKeys; + static std::map pubKeys; +}; diff --git a/Streams.cpp b/Streams.cpp new file mode 100644 index 0000000..f921762 --- /dev/null +++ b/Streams.cpp @@ -0,0 +1,68 @@ +#include "Streams.hpp" +#include + +using namespace std; + +shared_ptr getHostStream() +{ + static shared_ptr stream{ fopen("/dev/hidg0", "ab+"), [](FILE *f){ + fclose(f); + } }; + + if (!stream) + clog << "Stream is unavailable" << endl; + + return stream; +} + +shared_ptr getComHostStream() +{ + static shared_ptr stream{ fopen("comhost.txt", "wb"), [](FILE *f){ + clog << "Closing comhost stream" << endl; + fclose(f); + } }; + + if (!stream) + clog << "Stream is unavailable" << endl; + + return stream; +} + +shared_ptr getHostPacketStream() +{ + static shared_ptr stream{ fopen("hostpackets.txt", "wb"), [](FILE *f){ + clog << "Closing hostPackets stream" << endl; + fclose(f); + } }; + + if (!stream) + clog << "Stream is unavailable" << endl; + + return stream; +} + +shared_ptr getComDevStream() +{ + static shared_ptr stream{ fopen("comdev.txt", "wb"), [](FILE *f){ + clog << "Closing comdev stream" << endl; + fclose(f); + } }; + + if (!stream) + clog << "Stream is unavailable" << endl; + + return stream; +} + +shared_ptr getDevPacketStream() +{ + static shared_ptr stream{ fopen("devpackets.txt", "wb"), [](FILE *f){ + clog << "Closing devPackets stream" << endl; + fclose(f); + } }; + + if (!stream) + clog << "Stream is unavailable" << endl; + + return stream; +} diff --git a/Streams.hpp b/Streams.hpp new file mode 100644 index 0000000..2d67797 --- /dev/null +++ b/Streams.hpp @@ -0,0 +1,9 @@ +#pragma once +#include +#include + +std::shared_ptr getHostStream(); +std::shared_ptr getComHostStream(); +std::shared_ptr getHostPacketStream(); +std::shared_ptr getComDevStream(); +std::shared_ptr getDevPacketStream(); diff --git a/U2FMessage.cpp b/U2FMessage.cpp new file mode 100644 index 0000000..8ce7557 --- /dev/null +++ b/U2FMessage.cpp @@ -0,0 +1,87 @@ +#include "U2FMessage.hpp" +#include "Packet.hpp" +#include +#include +#include "Streams.hpp" + +using namespace std; + +U2FMessage U2FMessage::read() +{ + auto fPack = dynamic_pointer_cast(Packet::getPacket()); + + if (!fPack) + throw runtime_error{ "Failed to receive Init packet" }; + + const uint16_t messageSize = ((static_cast(fPack->bcnth) << 8u) + fPack->bcntl); + + clog << "Message on channel 0x" << hex << fPack->cid << dec << " has size: " << messageSize << endl; + + const uint16_t copyByteCount = min(static_cast(fPack->data.size()), messageSize); + + U2FMessage message{ fPack-> cid, fPack->cmd }; + message.data.assign(fPack->data.begin(), fPack->data.begin() + copyByteCount); + + uint8_t currSeq = 0; + + while (message.data.size() < messageSize) + { + auto newPack = dynamic_pointer_cast(Packet::getPacket()); + + if (!newPack) + throw runtime_error{ "Failed to receive Cont packet" }; + else if (newPack->seq != currSeq) + throw runtime_error{ "Packet out of sequence" }; + + const uint16_t remainingBytes = messageSize - message.data.size(); + clog << "Remaining bytes: " << remainingBytes << endl; + const uint16_t copyBytes = min(static_cast(newPack->data.size()), remainingBytes); + message.data.insert(message.data.end(), newPack->data.begin(), newPack->data.begin() + copyBytes); + + currSeq++; + } + + return message; +} + +void U2FMessage::write() +{ + fflush(getHostStream().get()); + const uint16_t bytesToWrite = this->data.size(); + uint16_t bytesWritten = 0; + + { + const uint8_t bcnth = bytesToWrite >> 8; + const uint8_t bcntl = bytesToWrite - (bcnth << 8); + + InitPacket p{}; + p.cid = cid; + p.cmd = cmd; + p.bcnth = bcnth; + p.bcntl = bcntl; + + { + uint16_t initialByteCount = min(static_cast(p.data.size()), static_cast(bytesToWrite - bytesWritten)); + copy(data.begin(), data.begin() + initialByteCount, p.data.begin()); + bytesWritten += initialByteCount; + } + + p.writePacket(); + } + + uint8_t seq = 0; + + while (bytesWritten != bytesToWrite) + { + ContPacket p{}; + p.cid = cid; + p.seq = seq; + uint16_t newByteCount = min(static_cast(p.data.size()), static_cast(bytesToWrite - bytesWritten)); + copy(data.begin() + bytesWritten, data.begin() + bytesWritten + newByteCount, p.data.begin()); + p.writePacket(); + seq++; + } + + auto stream = getHostStream().get(); + fflush(stream); +} diff --git a/U2FMessage.hpp b/U2FMessage.hpp new file mode 100644 index 0000000..a5b1784 --- /dev/null +++ b/U2FMessage.hpp @@ -0,0 +1,14 @@ +#pragma once +#include +#include + +struct U2FMessage +{ + uint32_t cid; + uint8_t cmd; + std::vector data; + + static U2FMessage read(); + + void write(); +}; diff --git a/U2F_CMD.hpp b/U2F_CMD.hpp new file mode 100644 index 0000000..0f1a954 --- /dev/null +++ b/U2F_CMD.hpp @@ -0,0 +1,10 @@ +#pragma once + +struct U2F_CMD +{ + protected: + U2F_CMD() = default; + + public: + ~U2F_CMD() = default; +}; //For polymorphic type casting diff --git a/U2F_Init_CMD.cpp b/U2F_Init_CMD.cpp new file mode 100644 index 0000000..6b7db01 --- /dev/null +++ b/U2F_Init_CMD.cpp @@ -0,0 +1,21 @@ +#include "U2F_Init_CMD.hpp" +#include +#include "U2FMessage.hpp" +#include "u2f.hpp" + +using namespace std; + +U2F_Init_CMD U2F_Init_CMD::get() +{ + const auto message = U2FMessage::read(); + + if (message.cmd != U2FHID_INIT) + throw runtime_error{ "Failed to get U2F Init message" }; + else if (message.data.size() != INIT_NONCE_SIZE) + throw runtime_error{ "Init nonce is incorrect size" }; + + U2F_Init_CMD cmd; + cmd.nonce = *reinterpret_cast(message.data.data()); + + return cmd; +} diff --git a/U2F_Init_CMD.hpp b/U2F_Init_CMD.hpp new file mode 100644 index 0000000..62cf408 --- /dev/null +++ b/U2F_Init_CMD.hpp @@ -0,0 +1,11 @@ +#pragma once +#include +#include "U2F_CMD.hpp" + +struct U2F_Init_CMD : U2F_CMD +{ + uint64_t nonce; + + public: + static U2F_Init_CMD get(); +}; diff --git a/U2F_Init_Response.hpp b/U2F_Init_Response.hpp new file mode 100644 index 0000000..b3b3109 --- /dev/null +++ b/U2F_Init_Response.hpp @@ -0,0 +1,31 @@ +#include +#include "U2FMessage.hpp" +#include "Field.hpp" + +struct U2F_Init_Response : U2F_CMD +{ + uint32_t cid; + uint64_t nonce; + uint8_t protocolVer; + uint8_t majorDevVer; + uint8_t minorDevVer; + uint8_t buildDevVer; + uint8_t capabilities; + + void write() + { + U2FMessage m{}; + m.cid = CID_BROADCAST; + m.cmd = U2FHID_INIT; + + m.data.insert(m.data.begin() + 0, FIELD(nonce)); + m.data.insert(m.data.begin() + 8, FIELD(cid)); + m.data.insert(m.data.begin() + 12, FIELD(protocolVer)); + m.data.insert(m.data.begin() + 13, FIELD(majorDevVer)); + m.data.insert(m.data.begin() + 14, FIELD(minorDevVer)); + m.data.insert(m.data.begin() + 15, FIELD(buildDevVer)); + m.data.insert(m.data.begin() + 16, FIELD(capabilities)); + + m.write(); + } +}; diff --git a/U2F_Msg_CMD.cpp b/U2F_Msg_CMD.cpp new file mode 100644 index 0000000..313813c --- /dev/null +++ b/U2F_Msg_CMD.cpp @@ -0,0 +1,107 @@ +#include "U2F_Msg_CMD.hpp" +#include "APDU.hpp" +#include "U2F_Register_APDU.hpp" +#include "U2FMessage.hpp" +#include "u2f.hpp" +#include "APDU.hpp" +#include + +using namespace std; + +uint32_t U2F_Msg_CMD::getLe(const uint32_t byteCount, vector bytes) +{ + if (byteCount > 3) + throw runtime_error{ "Too much data for command" }; + if (byteCount != 0) + { + //Le must be length of data in bytes + + switch (byteCount) + { + case 1: + return (bytes[0] == 0 ? 256 : bytes[0]); + case 2: + //Don't handle non-compliance with spec here + //This case is only possible if extended Lc used + //CBA + return (bytes[0] == 0 && bytes[1] == 0 ? 65536 : bytes[0] << 8 + bytes[1]); + case 3: + //Don't handle non-compliance with spec here + //This case is only possible if extended Lc not used + //CBA + if (bytes[0] != 0) + throw runtime_error{ "First byte of 3-byte Le should be 0"}; + return (bytes[1] == 0 & bytes[2] == 0 ? 65536 : bytes[1] << 8 + bytes[2]); + } + } + else + return 0; +} + +shared_ptr U2F_Msg_CMD::get() +{ + const auto message = U2FMessage::read(); + + if (message.cmd != U2FHID_MSG) + throw runtime_error{ "Failed to get U2F Msg message" }; + + U2F_Msg_CMD cmd{}; + auto &dat = message.data; + + cmd.cla = dat[0]; + cmd.ins = dat[1]; + cmd.p1 = dat[2]; + cmd.p2 = dat[3]; + + const uint32_t cBCount = dat.size() - 4; + + vector data{ dat.begin() + 4, dat.end() }; + auto startPtr = data.begin(), endPtr = data.end(); + + if (usesData.at(cmd.ins)) + { + clog << "Assuming command uses data" << endl; + clog << "First bytes are: " << static_cast(data[0]) << " " << static_cast(data[1]) << " " << static_cast(data[2]) << endl; + + if (cBCount == 0) + throw runtime_error{ "Invalid command - should have attached data" }; + + if (data[0] != 0) //1 byte length + { + cmd.lc = data[0]; + startPtr++; + } + else + { + cmd.lc = (data[1] << 8) + data[2]; + startPtr += 3; + } + + clog << "Got command uses " << cmd.lc << " bytes of data" << endl; + + endPtr = startPtr + cmd.lc; + + cmd.le = getLe(data.end() - endPtr, vector(endPtr, data.end())); + } + else + { + clog << "Assuming command does not use data" << endl; + cmd.lc = 0; + endPtr = startPtr; + cmd.le = getLe(cBCount, data); + } + + const auto dBytes = vector(startPtr, endPtr); + + if (cmd.ins == APDU::U2F_REG) + return make_shared(cmd, dBytes); +} + +void U2F_Msg_CMD::respond(){}; + +const map U2F_Msg_CMD::usesData = { + { U2F_REG, true }, + { U2F_AUTH, true }, + { U2F_VER, false } +}; + diff --git a/U2F_Msg_CMD.hpp b/U2F_Msg_CMD.hpp new file mode 100644 index 0000000..24bb64e --- /dev/null +++ b/U2F_Msg_CMD.hpp @@ -0,0 +1,27 @@ +#pragma once +#include "U2F_CMD.hpp" +#include +#include +#include +#include + +struct U2F_Msg_CMD : U2F_CMD +{ + uint8_t cla; + uint8_t ins; + uint8_t p1; + uint8_t p2; + uint32_t lc; + uint32_t le; + + const static std::map usesData; + + protected: + static uint32_t getLe(const uint32_t byteCount, std::vector bytes); + + public: + static std::shared_ptr get(); + + virtual void respond(); +}; + diff --git a/U2F_Register_APDU.cpp b/U2F_Register_APDU.cpp new file mode 100644 index 0000000..678ba99 --- /dev/null +++ b/U2F_Register_APDU.cpp @@ -0,0 +1,109 @@ +#include "U2F_Register_APDU.hpp" +#include +#include +#include "micro-ecc/uECC.h" +#include "Certificates.hpp" +#include +#include + +using namespace std; + +U2F_Register_APDU::U2F_Register_APDU(const U2F_Msg_CMD &msg, const vector &data) + : U2F_Msg_CMD{ msg } +{ + if (data.size() != 64) + throw runtime_error{ "Incorrect registration size" }; + else if (p1 != 0x00 || p2 != 0x00) + { + cerr << "p1: " << static_cast(p1) << ", p2: " << static_cast(p2) << endl; + //throw runtime_error{ "Invalid APDU parameters" }; + } + + copy(data.data() + 0, data.data() + 32, challengeP.begin()); + copy(data.data() + 32, data.data() + 64, appP.begin()); + + //Use crypto lib to generate keypair + + uint8_t pub[65]; + uint8_t priv[32]; + + //Unsure if necessary + //From github.com/pratikd650/Teensy_U2F/blob/master/Teensy_U2F.cpp + pub[0] = 0x04; + + uECC_make_key(pub + 1, priv, uECC_secp256r1()); + + Storage::PrivKey privKey{}; + copy(priv, end(priv), privKey.begin()); + Storage::PubKey pubKey; + copy(pub, end(pub), pubKey.begin()); + + this->keyH = Storage::appParams.size(); + Storage::appParams[this->keyH] = appP; + Storage::privKeys[this->keyH] = privKey; + Storage::pubKeys[this->keyH] = pubKey; +} + +//Ripped from https://github.com/pratikd650/Teensy_U2F/blob/master/Teensy_U2F.cpp +void appendSignatureAsDER(vector &response, const array &signature) { + response.push_back(0x30); // Start of ASN.1 SEQUENCE + response.push_back(68); //total length from (2 * (32 + 2)) to (2 * (33 + 2)) + auto &count = response.back(); + + // Loop twice - for R and S + for(unsigned int i = 0; i < 2; i++) { + unsigned int sigOffs = i * 32; + response.push_back(0x02); //header: integer + response.push_back(32); //32 byte + if (signature[sigOffs] > 0x7f) { // Integer needs to be represented in 2's completement notion + response.back()++; + response.push_back(0); // add leading 0, to indicate it is a positive number + count++; + } + copy(signature.begin() + sigOffs, signature.begin() + sigOffs + 32, back_inserter(response)); //R or S value + } +} + +void U2F_Register_APDU::respond() +{ + const auto appParam = Storage::appParams[this->keyH]; + const auto pubKey = Storage::pubKeys[this->keyH]; + const auto privKey = Storage::privKeys[this->keyH]; + + vector response; + response.push_back(0x05); + copy(pubKey.begin(), pubKey.end(), back_inserter(response)); + response.push_back(sizeof(this->keyH)); + auto fakeKeyHBytes = reinterpret_cast(&this->keyH); + copy(fakeKeyHBytes, fakeKeyHBytes + 4, back_inserter(response)); + copy(attestCert, end(attestCert), back_inserter(response)); + + //Gen signature + array digest; + { + mbedtls_sha256_context shaContext; + + mbedtls_sha256_init(&shaContext); + mbedtls_sha256_starts(&shaContext, 0); + + uint8_t byteReserved = 0; + mbedtls_sha256_update(&shaContext, reinterpret_cast(&byteReserved), 1); + + mbedtls_sha256_update(&shaContext, reinterpret_cast(appParam.data()), appParam.size()); + + mbedtls_sha256_update(&shaContext, reinterpret_cast(challengeP.data()), challengeP.size()); + + mbedtls_sha256_update(&shaContext, reinterpret_cast(&keyH), sizeof(keyH)); + + mbedtls_sha256_update(&shaContext, reinterpret_cast(pubKey.data()), pubKey.size()); + + mbedtls_sha256_finish(&shaContext, reinterpret_cast(digest.data())); + mbedtls_sha256_free(&shaContext); + } + + array signature; + uECC_sign(attestPrivKey, digest.data(), digest.size(), signature.data(), uECC_secp256r1()); + + //Append signature as DER + appendSignatureAsDER(response, signature); +} diff --git a/U2F_Register_APDU.hpp b/U2F_Register_APDU.hpp new file mode 100644 index 0000000..e28faef --- /dev/null +++ b/U2F_Register_APDU.hpp @@ -0,0 +1,16 @@ +#pragma once +#include "U2F_Msg_CMD.hpp" +#include "Storage.hpp" + +struct U2F_Register_APDU : U2F_Msg_CMD +{ + std::array challengeP; + Storage::AppParam appP; + Storage::KeyHandle keyH; + + public: + U2F_Register_APDU(const U2F_Msg_CMD &msg, const std::vector &data); + + void respond(); +}; + diff --git a/micro-ecc b/micro-ecc new file mode 160000 index 0000000..601bd11 --- /dev/null +++ b/micro-ecc @@ -0,0 +1 @@ +Subproject commit 601bd11062c551b108adbb43ba99f199b840777c diff --git a/monitor.cpp b/monitor.cpp index ec1f1dd..602ded9 100644 --- a/monitor.cpp +++ b/monitor.cpp @@ -5,349 +5,23 @@ #include #include #include +#include +#include #include "u2f.hpp" +#include "Constants.hpp" +#include "U2FMessage.hpp" +#include "U2F_CMD.hpp" +#include "Storage.hpp" +#include "U2F_Init_CMD.hpp" +#include "U2F_Init_Response.hpp" +#include "U2F_Msg_CMD.hpp" using namespace std; -const constexpr uint16_t packetSize = 64; - -struct Packet -{ - uint32_t cid; - - protected: - Packet() = default; - virtual void writePacket(); - - public: - static shared_ptr getPacket(); - virtual ~Packet() = default; -}; - - -struct InitPacket : Packet -{ - uint8_t cmd; - uint8_t bcnth; - uint8_t bcntl; - array data{}; - - public: - InitPacket() = default; - static shared_ptr getPacket(const uint32_t rCID, const uint8_t rCMD); - void writePacket() override; -}; - -struct ContPacket : Packet -{ - uint8_t seq; - array data{}; - - public: - ContPacket() = default; - static shared_ptr getPacket(const uint32_t rCID, const uint8_t rSeq); - void writePacket() override; -}; - -shared_ptr getHostStream() -{ - static shared_ptr stream{ fopen("/dev/hidg0", "ab+"), [](FILE *f){ - fclose(f); - } }; - - if (!stream) - clog << "Stream is unavailable" << endl; - - return stream; -} - -shared_ptr getComHostStream() -{ - static shared_ptr stream{ fopen("comhost.txt", "wb"), [](FILE *f){ - clog << "Closing comhost stream" << endl; - fclose(f); - } }; - - if (!stream) - clog << "Stream is unavailable" << endl; - - return stream; -} - -shared_ptr getComDevStream() -{ - static shared_ptr stream{ fopen("comdev.txt", "wb"), [](FILE *f){ - clog << "Closing comdev stream" << endl; - fclose(f); - } }; - - if (!stream) - clog << "Stream is unavailable" << endl; - - return stream; -} - -vector readBytes(const size_t count) -{ - vector bytes(count); - - size_t readByteCount; - - do - { - readByteCount = fread(bytes.data(), 1, count, getHostStream().get()); - fwrite(bytes.data(), 1, bytes.size(), getComHostStream().get()); - } while (readByteCount == 0); - - clog << "Read " << readByteCount << " bytes" << endl; - - if (readByteCount != count) - throw runtime_error{ "Failed to read sufficient bytes" }; - - return bytes; -} - -shared_ptr InitPacket::getPacket(const uint32_t rCID, const uint8_t rCMD) -{ - auto p = make_shared(); - p->cid = rCID; - p->cmd = rCMD; - p->bcnth = readBytes(1)[0]; - p->bcntl = readBytes(1)[0]; - - const auto dataBytes = readBytes(p->data.size()); - copy(dataBytes.begin(), dataBytes.end(), p->data.data()); - - clog << "Fully read init packet" << endl; - return p; -} - -shared_ptr ContPacket::getPacket(const uint32_t rCID, const uint8_t rSeq) -{ - auto p = make_shared(); - p->cid = rCID; - p->seq = rSeq; - - const auto dataBytes = readBytes(p->data.size()); - copy(dataBytes.begin(), dataBytes.end(), p->data.data()); - - clog << "Fully read cont packet" << endl; - return p; -} - -shared_ptr Packet::getPacket() -{ - const uint32_t cid = *reinterpret_cast(readBytes(4).data()); - uint8_t b = readBytes(1)[0]; - - clog << "Packet read 2nd byte as " << static_cast(b) << endl; - - if (b && TYPE_MASK) - { - //Init packet - return InitPacket::getPacket(cid, b); - } - else - { - //Cont packet - return ContPacket::getPacket(cid, b); - } -} - -void Packet::writePacket() -{ - const uint8_t reportID = FIDO_USAGE_DATA_OUT; - auto hostStream = getHostStream().get(); - auto devStream = getComDevStream().get(); - - fwrite(&reportID, 1, 1, hostStream); - fwrite(&cid, 4, 1, hostStream); - fwrite(&cid, 4, 1, devStream); -} - -void InitPacket::writePacket() -{ - Packet::writePacket(); - auto hostStream = getHostStream().get(); - auto devStream = getComDevStream().get(); - - fwrite(&cmd, 1, 1, hostStream); - fwrite(&bcnth, 1, 1, hostStream); - fwrite(&bcntl, 1, 1, hostStream); - fwrite(data.data(), 1, data.size(), hostStream); - fwrite(&cmd, 1, 1, devStream); - fwrite(&bcnth, 1, 1, devStream); - fwrite(&bcntl, 1, 1, devStream); - fwrite(data.data(), 1, data.size(), devStream); - - perror(nullptr); - clog << "Fully wrote init packet" << endl; -} - -void ContPacket::writePacket() -{ - Packet::writePacket(); - auto hostStream = getHostStream().get(); - auto devStream = getComDevStream().get(); - - fwrite(&seq, 1, 1, hostStream); - fwrite(data.data(), 1, data.size(), hostStream); - fwrite(&seq, 1, 1, devStream); - fwrite(data.data(), 1, data.size(), devStream); - - perror(nullptr); - clog << "Fully wrote cont packet" << endl; -} - -struct U2FMessage -{ - uint32_t cid; - uint8_t cmd; - vector data; - - static U2FMessage read() - { - auto fPack = dynamic_pointer_cast(Packet::getPacket()); - - if (!fPack) - throw runtime_error{ "Failed to receive Init packet" }; - - const uint16_t messageSize = ((static_cast(fPack->bcnth) << 8u) + fPack->bcntl); - - clog << "Message has size: " << messageSize << endl; - - const uint16_t copyByteCount = min(static_cast(fPack->data.size()), messageSize); - - U2FMessage message{ fPack-> cid, fPack->cmd }; - message.data.assign(fPack->data.begin(), fPack->data.begin() + copyByteCount); - - uint8_t currSeq = 0; - - while (message.data.size() < messageSize) - { - auto newPack = dynamic_pointer_cast(Packet::getPacket()); - - if (!newPack) - throw runtime_error{ "Failed to receive Cont packet" }; - else if (newPack->seq != currSeq) - throw runtime_error{ "Packet out of sequence" }; - - const uint16_t remainingBytes = messageSize - message.data.size(); - const uint16_t copyBytes = min(static_cast(newPack->data.size()), remainingBytes); - message.data.insert(message.data.end(), newPack->data.begin(), newPack->data.begin() + copyBytes); - - currSeq++; - } - - return message; - } - - void write() - { - fflush(getHostStream().get()); - const uint16_t bytesToWrite = this->data.size(); - uint16_t bytesWritten = 0; - - { - const uint8_t bcnth = bytesToWrite >> 8; - const uint8_t bcntl = bytesToWrite - (bcnth << 8); - - InitPacket p{}; - p.cid = cid; - p.cmd = cmd; - p.bcnth = bcnth; - p.bcntl = bcntl; - - { - uint16_t initialByteCount = min(static_cast(p.data.size()), static_cast(bytesToWrite - bytesWritten)); - copy(data.begin(), data.begin() + initialByteCount, p.data.begin()); - bytesWritten += initialByteCount; - } - - p.writePacket(); - } - - uint8_t seq = 0; - - while (bytesWritten != bytesToWrite) - { - ContPacket p{}; - p.cid = cid; - p.seq = seq; - uint16_t newByteCount = min(static_cast(p.data.size()), static_cast(bytesToWrite - bytesWritten)); - copy(data.begin() + bytesWritten, data.begin() + bytesWritten + newByteCount, p.data.begin()); - p.writePacket(); - seq++; - } - - auto stream = getHostStream().get(); - fflush(stream); - } -}; - -struct U2F_CMD -{ - protected: - U2F_CMD() = default; - - public: - ~U2F_CMD() = default; -}; //For polymorphic type casting - -struct U2F_Init_CMD : U2F_CMD -{ - uint64_t nonce; - - public: - static U2F_Init_CMD get() - { - const auto message = U2FMessage::read(); - - if (message.cmd != U2FHID_INIT) - throw runtime_error{ "Failed to get U2F Init message" }; - else if (message.data.size() != INIT_NONCE_SIZE) - throw runtime_error{ "Init nonce is incorrect size" }; - - U2F_Init_CMD cmd; - cmd.nonce = *reinterpret_cast(message.data.data()); - - return cmd; - } -}; - -#define FIELD(name) reinterpret_cast(&name), (reinterpret_cast(&name) + sizeof(name)) - -struct U2F_Init_Response : U2F_CMD -{ - uint32_t cid; - uint64_t nonce; - uint8_t protocolVer; - uint8_t majorDevVer; - uint8_t minorDevVer; - uint8_t buildDevVer; - uint8_t capabilities; - - void write() - { - U2FMessage m{}; - m.cid = CID_BROADCAST; - m.cmd = U2FHID_INIT; - - m.data.insert(m.data.begin() + 0, FIELD(nonce)); - m.data.insert(m.data.begin() + 8, FIELD(cid)); - m.data.insert(m.data.begin() + 12, FIELD(protocolVer)); - m.data.insert(m.data.begin() + 13, FIELD(majorDevVer)); - m.data.insert(m.data.begin() + 14, FIELD(minorDevVer)); - m.data.insert(m.data.begin() + 15, FIELD(buildDevVer)); - m.data.insert(m.data.begin() + 16, FIELD(capabilities)); - - m.write(); - } -}; - - int main() { + Storage::init(); + auto initFrame = U2F_Init_CMD::get(); U2F_Init_Response resp{}; @@ -361,8 +35,16 @@ int main() resp.write(); - /*U2FMessage m = m.read(); + auto m = U2F_Msg_CMD::get(); + + cout << "U2F CMD ins: " << static_cast(m->ins) << endl; + + /*auto m = U2FMessage::read(); + + cout << "U2F CMD: " << static_cast(m.cmd) << endl; for (const auto d : m.data) clog << static_cast(d) << endl;*/ + + Storage::save(); }