michaelkuc6/U2FDevice
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Added registration.

Browse Source 
Currently doesn't return any valid result, hence host believes device
failed to register, however, insecure storage of keys does occur.
This commit is contained in:
Michael Kuc
2018-07-31 00:09:25 +00:00
parent 03e105bbcf
commit 86af080ba6
39 changed files with 1136 additions and 339 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
.gitignore vendored
View File

@@ -1,4 +1,9 @@
deleteme
SOut.txt
comdev.txt
comhost.txt
libuECC.o
monitor
obj/*
U2F_Priv_Keys.txt
devpackets.txt
hostpackets.txt

3
.gitmodules vendored Normal file
View File

@@ -0,0 +1,3 @@
[submodule "micro-ecc"]
path = micro-ecc
url = https://github.com/kmackay/micro-ecc.git

8
APDU.hpp Normal file
View File

@@ -0,0 +1,8 @@
#pragma once
enum APDU
{
U2F_REG = 0x01,
U2F_AUTH = 0x02,
U2F_VER = 0x03
};

19
Base64.hpp Normal file
View File

@@ -0,0 +1,19 @@
#pragma once
template <typename InContainer, typename OutContainer>
void b64encode(const InContainer &iContainer, OutContainer &oContainer);
template <typename InContainer, typename OutContainer>
void b64decode(const InContainer &container, OutContainer &oContainer);
template <typename InContainer, typename Elem, size_t count>
void b64encode(const InContainer &iContainer, std::array<Elem, count> &oArr);
template <typename InContainer, typename Elem, size_t count>
void b64decode(const InContainer &iContainer, std::array<Elem, count> &oArr);
template <typename InContainerIter, typename OutContainerIter>
void b64encode(const InContainerIter beginIter, const InContainerIter endIter, OutContainerIter oBeginIter);
template <typename InContainerIter, typename OutContainerIter>
void b64decode(const InContainerIter beginIter, const InContainerIter endIter, OutContainerIter oBeginIter);

53
Base64.tpp Normal file
View File

@@ -0,0 +1,53 @@
#include "Base64.hpp"
#include <sstream>
#include <b64/encode.h>
#include <b64/decode.h>
#include <array>
#include <algorithm>
template <typename InContainer, typename OutContainer>
void b64encode(const InContainer &iContainer, OutContainer &oContainer)
{
b64encode(std::begin(iContainer), std::end(iContainer), std::back_inserter(oContainer));
}
template <typename InContainer, typename OutContainer>
void b64decode(const InContainer &iContainer, OutContainer &oContainer)
{
b64decode(std::begin(iContainer), std::end(iContainer), std::back_inserter(oContainer));
}
template <typename InContainer, typename Elem, size_t count>
void b64encode(const InContainer &iContainer, std::array<Elem, count> &oArr)
{
b64encode(std::begin(iContainer), std::end(iContainer), std::begin(oArr));
}
template <typename InContainer, typename Elem, size_t count>
void b64decode(const InContainer &iContainer, std::array<Elem, count> &oArr)
{
b64decode(std::begin(iContainer), std::end(iContainer), std::begin(oArr));
}
template <typename InContainerIter, typename OutContainerIter>
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 <typename InContainerIter, typename OutContainerIter>
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);
}

72
Certificates.cpp Normal file
View File

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

5
Certificates.hpp Normal file
View File

@@ -0,0 +1,5 @@
#include <cstdint>
extern uint8_t attestCert[557];
extern uint8_t attestPrivKey[32];
extern uint8_t attestPubKey[65];

4
Constants.hpp Normal file
View File

@@ -0,0 +1,4 @@
#pragma once
#include <cstdint>
const constexpr uint16_t packetSize = 64;

3
Field.hpp Normal file
View File

@@ -0,0 +1,3 @@
#pragma once
#define FIELD(name) reinterpret_cast<uint8_t*>(&name), (reinterpret_cast<uint8_t*>(&name) + sizeof(name))

26
IO.cpp Normal file
View File

@@ -0,0 +1,26 @@
#include "IO.hpp"
#include "Streams.hpp"
#include <iostream>
using namespace std;
vector<uint8_t> readBytes(const size_t count)
{
vector<uint8_t> 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;
}

6
IO.hpp Normal file
View File

@@ -0,0 +1,6 @@
#pragma once
#include <cstddef>
#include <cstdint>
#include <vector>
std::vector<uint8_t> readBytes(const size_t count);

BIN
Keys/certificate.der Normal file
View File

Binary file not shown.

5
Keys/ecprivkey.pem Normal file
View File

@@ -0,0 +1,5 @@
-----BEGIN EC PRIVATE KEY-----
MHcCAQEEIH69kQVagJ825S/g0KljDIYEsQTj0fvQg8cuLzS21qSyoAoGCCqGSM49
AwEHoUQDQgAEMkHDuJaX2JBmQYiW1HO2N/eFKa87FQ+DYWfqybLbgrMsmWCKmHzU
BKCSIgWq93qRAgPdFYiHaibp7s+ZsWbAAQ==
-----END EC PRIVATE KEY-----

4
Keys/ecpubkey.pem Normal file
View File

@@ -0,0 +1,4 @@
-----BEGIN PUBLIC KEY-----
MFkwEwYHKoZIzj0CAQYIKoZIzj0DAQcDQgAEMkHDuJaX2JBmQYiW1HO2N/eFKa87
FQ+DYWfqybLbgrMsmWCKmHzUBKCSIgWq93qRAgPdFYiHaibp7s+ZsWbAAQ==
-----END PUBLIC KEY-----

1
Keys/privkey.der Normal file
View File

@@ -0,0 +1 @@
~<7E><>Z<><5A>6<EFBFBD>/<2F>Щc <0C><04><04><><EFBFBD>Ѓ<EFBFBD>./4<>֤<EFBFBD>

1
Keys/pubkey.der Normal file
View File

@@ -0,0 +1 @@
2Aø<41><C3B8>ؐfA<66><41><EFBFBD>s<EFBFBD>7<EFBFBD><37>)<29>;<0F>ag<61>ɲۂ<C9B2>,<2C>`<60><>|<7C><04><>"<05><>z<EFBFBD><03><15><>j&<26><>ϙ<EFBFBD>f<EFBFBD>

14
Keys/server.pem Normal file
View File

@@ -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-----

28
Makefile Executable file
View File

@@ -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

109
Packet.cpp Normal file
View File

@@ -0,0 +1,109 @@
#include "Packet.hpp"
#include "IO.hpp"
#include "u2f.hpp"
#include <cstring>
#include <iostream>
#include "Streams.hpp"
using namespace std;
shared_ptr<InitPacket> InitPacket::getPacket(const uint32_t rCID, const uint8_t rCMD)
{
auto p = make_shared<InitPacket>();
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> ContPacket::getPacket(const uint32_t rCID, const uint8_t rSeq)
{
auto p = make_shared<ContPacket>();
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> Packet::getPacket()
{
const uint32_t cid = *reinterpret_cast<uint32_t*>(readBytes(4).data());
uint8_t b = readBytes(1)[0];
clog << "Packet read 2nd byte as " << static_cast<uint16_t>(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;
}

43
Packet.hpp Normal file
View File

@@ -0,0 +1,43 @@
#pragma once
#include <cstdint>
#include <memory>
#include <array>
#include "Constants.hpp"
struct Packet
{
uint32_t cid;
uint8_t buf[packetSize];
protected:
Packet() = default;
virtual void writePacket();
public:
static std::shared_ptr<Packet> getPacket();
virtual ~Packet() = default;
};
struct InitPacket : Packet
{
uint8_t cmd;
uint8_t bcnth;
uint8_t bcntl;
std::array<uint8_t, packetSize - 7> data{};
public:
InitPacket() = default;
static std::shared_ptr<InitPacket> getPacket(const uint32_t rCID, const uint8_t rCMD);
void writePacket() override;
};
struct ContPacket : Packet
{
uint8_t seq;
std::array<uint8_t, packetSize - 5> data{};
public:
ContPacket() = default;
static std::shared_ptr<ContPacket> getPacket(const uint32_t rCID, const uint8_t rSeq);
void writePacket() override;
};

45
Readme.AttestationCertificateGeneration.txt Normal file
View File

@@ -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.

8
Readme.txt Normal file
View File

@@ -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

27
Scripts/uECC-Makefile Executable file
View File

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

90
Storage.cpp Normal file
View File

@@ -0,0 +1,90 @@
#include "Storage.hpp"
#include <exception>
#include <sstream>
#include "Base64.tpp"
#include <iostream>
using namespace std;
std::string Storage::filename{};
std::map<Storage::KeyHandle, Storage::AppParam> Storage::appParams{};
std::map<Storage::KeyHandle, Storage::PrivKey> Storage::privKeys{};
std::map<Storage::KeyHandle, Storage::PubKey> 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;
}
}

25
Storage.hpp Normal file
View File

@@ -0,0 +1,25 @@
#include <map>
#include <array>
#include <string>
#include <fstream>
class Storage
{
public:
using KeyHandle = uint32_t;
using AppParam = std::array<uint8_t, 32>;
using PrivKey = std::array<uint8_t, 32>;
using PubKey = std::array<uint8_t, 65>;
protected:
Storage() = default;
static std::string filename;
public:
static void init(const std::string &dirPrefix = "");
static void save();
static std::map<KeyHandle, AppParam> appParams;
static std::map<KeyHandle, PrivKey> privKeys;
static std::map<KeyHandle, PubKey> pubKeys;
};

68
Streams.cpp Normal file
View File

@@ -0,0 +1,68 @@
#include "Streams.hpp"
#include <iostream>
using namespace std;
shared_ptr<FILE> getHostStream()
{
static shared_ptr<FILE> stream{ fopen("/dev/hidg0", "ab+"), [](FILE *f){
fclose(f);
} };
if (!stream)
clog << "Stream is unavailable" << endl;
return stream;
}
shared_ptr<FILE> getComHostStream()
{
static shared_ptr<FILE> 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<FILE> getHostPacketStream()
{
static shared_ptr<FILE> 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<FILE> getComDevStream()
{
static shared_ptr<FILE> 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<FILE> getDevPacketStream()
{
static shared_ptr<FILE> stream{ fopen("devpackets.txt", "wb"), [](FILE *f){
clog << "Closing devPackets stream" << endl;
fclose(f);
} };
if (!stream)
clog << "Stream is unavailable" << endl;
return stream;
}

9
Streams.hpp Normal file
View File

@@ -0,0 +1,9 @@
#pragma once
#include <cstdio>
#include <memory>
std::shared_ptr<FILE> getHostStream();
std::shared_ptr<FILE> getComHostStream();
std::shared_ptr<FILE> getHostPacketStream();
std::shared_ptr<FILE> getComDevStream();
std::shared_ptr<FILE> getDevPacketStream();

87
U2FMessage.cpp Normal file
View File

@@ -0,0 +1,87 @@
#include "U2FMessage.hpp"
#include "Packet.hpp"
#include <stdexcept>
#include <iostream>
#include "Streams.hpp"
using namespace std;
U2FMessage U2FMessage::read()
{
auto fPack = dynamic_pointer_cast<InitPacket>(Packet::getPacket());
if (!fPack)
throw runtime_error{ "Failed to receive Init packet" };
const uint16_t messageSize = ((static_cast<uint16_t>(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<uint16_t>(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<ContPacket>(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<uint16_t>(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<uint16_t>(p.data.size()), static_cast<uint16_t>(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<uint16_t>(p.data.size()), static_cast<uint16_t>(bytesToWrite - bytesWritten));
copy(data.begin() + bytesWritten, data.begin() + bytesWritten + newByteCount, p.data.begin());
p.writePacket();
seq++;
}
auto stream = getHostStream().get();
fflush(stream);
}

14
U2FMessage.hpp Normal file
View File

@@ -0,0 +1,14 @@
#pragma once
#include <cstdint>
#include <vector>
struct U2FMessage
{
uint32_t cid;
uint8_t cmd;
std::vector<uint8_t> data;
static U2FMessage read();
void write();
};

10
U2F_CMD.hpp Normal file
View File

@@ -0,0 +1,10 @@
#pragma once
struct U2F_CMD
{
protected:
U2F_CMD() = default;
public:
~U2F_CMD() = default;
}; //For polymorphic type casting

21
U2F_Init_CMD.cpp Normal file
View File

@@ -0,0 +1,21 @@
#include "U2F_Init_CMD.hpp"
#include <stdexcept>
#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<const uint64_t*>(message.data.data());
return cmd;
}

11
U2F_Init_CMD.hpp Normal file
View File

@@ -0,0 +1,11 @@
#pragma once
#include <cstdint>
#include "U2F_CMD.hpp"
struct U2F_Init_CMD : U2F_CMD
{
uint64_t nonce;
public:
static U2F_Init_CMD get();
};

31
U2F_Init_Response.hpp Normal file
View File

@@ -0,0 +1,31 @@
#include <cstdint>
#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();
}
};

107
U2F_Msg_CMD.cpp Normal file
View File

@@ -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 <iostream>
using namespace std;
uint32_t U2F_Msg_CMD::getLe(const uint32_t byteCount, vector<uint8_t> 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> 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<uint8_t> 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<uint16_t>(data[0]) << " " << static_cast<uint16_t>(data[1]) << " " << static_cast<uint16_t>(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<uint8_t>(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<uint8_t>(startPtr, endPtr);
if (cmd.ins == APDU::U2F_REG)
return make_shared<U2F_Register_APDU>(cmd, dBytes);
}
void U2F_Msg_CMD::respond(){};
const map<uint8_t, bool> U2F_Msg_CMD::usesData = {
{ U2F_REG, true },
{ U2F_AUTH, true },
{ U2F_VER, false }
};

27
U2F_Msg_CMD.hpp Normal file
View File

@@ -0,0 +1,27 @@
#pragma once
#include "U2F_CMD.hpp"
#include <cstdint>
#include <map>
#include <vector>
#include <memory>
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<uint8_t, bool> usesData;
protected:
static uint32_t getLe(const uint32_t byteCount, std::vector<uint8_t> bytes);
public:
static std::shared_ptr<U2F_Msg_CMD> get();
virtual void respond();
};

109
U2F_Register_APDU.cpp Normal file
View File

@@ -0,0 +1,109 @@
#include "U2F_Register_APDU.hpp"
#include <exception>
#include <cstring>
#include "micro-ecc/uECC.h"
#include "Certificates.hpp"
#include <mbedtls/sha256.h>
#include <iostream>
using namespace std;
U2F_Register_APDU::U2F_Register_APDU(const U2F_Msg_CMD &msg, const vector<uint8_t> &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<uint32_t>(p1) << ", p2: " << static_cast<uint32_t>(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<uint8_t> &response, const array<uint8_t, 64> &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<uint8_t> response;
response.push_back(0x05);
copy(pubKey.begin(), pubKey.end(), back_inserter(response));
response.push_back(sizeof(this->keyH));
auto fakeKeyHBytes = reinterpret_cast<uint8_t *>(&this->keyH);
copy(fakeKeyHBytes, fakeKeyHBytes + 4, back_inserter(response));
copy(attestCert, end(attestCert), back_inserter(response));
//Gen signature
array<uint8_t, 32> digest;
{
mbedtls_sha256_context shaContext;
mbedtls_sha256_init(&shaContext);
mbedtls_sha256_starts(&shaContext, 0);
uint8_t byteReserved = 0;
mbedtls_sha256_update(&shaContext, reinterpret_cast<unsigned char*>(&byteReserved), 1);
mbedtls_sha256_update(&shaContext, reinterpret_cast<const unsigned char*>(appParam.data()), appParam.size());
mbedtls_sha256_update(&shaContext, reinterpret_cast<unsigned char*>(challengeP.data()), challengeP.size());
mbedtls_sha256_update(&shaContext, reinterpret_cast<unsigned char*>(&keyH), sizeof(keyH));
mbedtls_sha256_update(&shaContext, reinterpret_cast<const unsigned char*>(pubKey.data()), pubKey.size());
mbedtls_sha256_finish(&shaContext, reinterpret_cast<unsigned char*>(digest.data()));
mbedtls_sha256_free(&shaContext);
}
array<uint8_t, 64> signature;
uECC_sign(attestPrivKey, digest.data(), digest.size(), signature.data(), uECC_secp256r1());
//Append signature as DER
appendSignatureAsDER(response, signature);
}

16
U2F_Register_APDU.hpp Normal file
View File

@@ -0,0 +1,16 @@
#pragma once
#include "U2F_Msg_CMD.hpp"
#include "Storage.hpp"
struct U2F_Register_APDU : U2F_Msg_CMD
{
std::array<uint8_t, 32> challengeP;
Storage::AppParam appP;
Storage::KeyHandle keyH;
public:
U2F_Register_APDU(const U2F_Msg_CMD &msg, const std::vector<uint8_t> &data);
void respond();
};

1
micro-ecc Submodule

Submodule micro-ecc added at 601bd11062

358
monitor.cpp
View File

@@ -5,349 +5,23 @@
#include <vector>
#include <algorithm>
#include <iostream>
#include <cstring>
#include <map>
#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<Packet> getPacket();
virtual ~Packet() = default;
};
struct InitPacket : Packet
{
uint8_t cmd;
uint8_t bcnth;
uint8_t bcntl;
array<uint8_t, packetSize - 7> data{};
public:
InitPacket() = default;
static shared_ptr<InitPacket> getPacket(const uint32_t rCID, const uint8_t rCMD);
void writePacket() override;
};
struct ContPacket : Packet
{
uint8_t seq;
array<uint8_t, packetSize - 5> data{};
public:
ContPacket() = default;
static shared_ptr<ContPacket> getPacket(const uint32_t rCID, const uint8_t rSeq);
void writePacket() override;
};
shared_ptr<FILE> getHostStream()
{
static shared_ptr<FILE> stream{ fopen("/dev/hidg0", "ab+"), [](FILE *f){
fclose(f);
} };
if (!stream)
clog << "Stream is unavailable" << endl;
return stream;
}
shared_ptr<FILE> getComHostStream()
{
static shared_ptr<FILE> 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<FILE> getComDevStream()
{
static shared_ptr<FILE> stream{ fopen("comdev.txt", "wb"), [](FILE *f){
clog << "Closing comdev stream" << endl;
fclose(f);
} };
if (!stream)
clog << "Stream is unavailable" << endl;
return stream;
}
vector<uint8_t> readBytes(const size_t count)
{
vector<uint8_t> 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> InitPacket::getPacket(const uint32_t rCID, const uint8_t rCMD)
{
auto p = make_shared<InitPacket>();
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> ContPacket::getPacket(const uint32_t rCID, const uint8_t rSeq)
{
auto p = make_shared<ContPacket>();
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> Packet::getPacket()
{
const uint32_t cid = *reinterpret_cast<uint32_t*>(readBytes(4).data());
uint8_t b = readBytes(1)[0];
clog << "Packet read 2nd byte as " << static_cast<uint16_t>(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<uint8_t> data;
static U2FMessage read()
{
auto fPack = dynamic_pointer_cast<InitPacket>(Packet::getPacket());
if (!fPack)
throw runtime_error{ "Failed to receive Init packet" };
const uint16_t messageSize = ((static_cast<uint16_t>(fPack->bcnth) << 8u) + fPack->bcntl);
clog << "Message has size: " << messageSize << endl;
const uint16_t copyByteCount = min(static_cast<uint16_t>(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<ContPacket>(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<uint16_t>(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<uint16_t>(p.data.size()), static_cast<uint16_t>(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<uint16_t>(p.data.size()), static_cast<uint16_t>(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<const uint64_t*>(message.data.data());
return cmd;
}
};
#define FIELD(name) reinterpret_cast<uint8_t*>(&name), (reinterpret_cast<uint8_t*>(&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<uint32_t>(m->ins) << endl;
/*auto m = U2FMessage::read();
cout << "U2F CMD: " << static_cast<uint32_t>(m.cmd) << endl;
for (const auto d : m.data)
clog << static_cast<uint16_t>(d) << endl;*/
Storage::save();
}